初值赋值

propagation 初值赋值： 位姿 translation 部分： r0=t{TWS} 位姿转换成四元数： qWS0=q{TWS} 位姿旋转部分： CWS0=C{TWS}

积分初值: 四元数积分： Δq=(1,0,0,0) 旋转矩阵积分： ∫C=0(3,3) 旋转矩阵双重积分： ∬C=0(3,3) 加速度积分： ∫a=(0,0,0) 加速度双重积分： ∬a=(0,0,0)

定义：

// cross matrix accumulatrion Eigen::Matrix3d cross = Eigen::Matrix3d::Zero();

Mcross=0(3,3)

子雅各比矩阵初始化 角速度对角速度偏置偏导： dαdbg=0(3,3) 速度对角速度偏置偏导： dvdbg=0(3,3) 位移对角速度偏置偏导： dpdbg=0(3,3)

// the Jacobian of the increment (w/o biases) Eigen::Matrix<double,15,15> P_delta = Eigen::Matrix<double,15,15>::Zero();

增量变量 δχ 偏导矩阵初始化： Pδ=I(15,15)

从最开始到当前次积分的时间间隔： Δt=0

for

it 个角速度测量： Sω0 it 个加速度测量： Sa0 it+1 个IMU 测量： Sω1 it+1 个加速度测量： Sa1

t0 到 it1 时间间隔： dt 从配置文件中读取的 gyro noise density [rad/s/sqrt(Hz)]： σgc 从配置文件中读取的 accelerometer noise density [m/s^2/sqrt(Hz)]： σac

四元数积分： dq 角速度设为时间 t0 和 t1 平均值： Sω=0.5∗(Sω0+Sω1)−bg dqv=sin(||12Sω dt||)∗(12Sω dt) dqw=cos(||12Sω dt||) dq=(dqv,dqw) 当前次四元数积分： Δq1=Δq∗dq

四元数转化成旋转矩阵： C=M{Δq} 四元数转化成旋转矩阵： C1=M{Δq1} 加速度设为时间 t0 和 t1 平均值： Sa=0.5∗(Sa0+Sa1)−ba ∫′C=∫C+0.5∗(C+C1)∗dt ∫′a=∫a+0.5∗(C+C1)∗Sa∗dt ∬C=∬C+∫C∗dt+0.25∗(C+C1)∗dt∗dt ∬a=∬a+∫a∗dt+0.25∗(C+C1)∗Sa∗dt∗dt

dαdbg=dαdbg+C1∗dt Mcross1=C{dq−1}∗Mcross+Jr{Sω∗dt}∗dt dvdbg′=dvdbg+0.5∗dt∗(C∗[Sa]×∗Mcross+C1∗[Sa]×∗Mcross1) dpdbg=dpdbg+dt∗dvdbg+0.25∗dt∗dt∗(C∗[Sa]×∗Mcross+C1∗[Sa]×∗Mcross1)

covariance propagation

Fδ=I(15,15) Fδ(0:2,3:5)=−[∫a∗dt+0.25∗(C+C1)∗Sa∗dt∗dt]× Fδ(0:2,6:8)=I(3,3)∗dt Fδ(0:2,9:11)=dt∗dvdbg+0.25∗dt∗dt∗(C∗[Sa]×∗Mcross+C1∗[Sa]×∗Mcross1) Fδ(3:5,9:11)=−dt∗C1 Fδ(6:8,3:5)=−[0.5∗(C+C1)∗Sa∗dt]× Fδ(6:8,9:11)=0.5∗dt∗(C∗[aS]×∗Mcross+C1∗[aS]×∗Mcross1) Fδ(6:8,12:15)=−0.5∗(C+C1)∗dt

Fδ=⎧⎩⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪00000−[∫a∗dt+0.25∗(C+C1)∗Sa∗dt∗dt]×0−[0.5∗(C+C1)∗Sa∗dt]×00I(3,3)∗dt0000dt∗dvdbg+0.25∗dt∗dt∗(C∗[Sa]×∗Mcross+C1∗[Sa]×∗Mcross1)−dt∗C10.5∗dt∗(C∗[aS]×∗Mcross+C1∗[aS]×∗Mcross1)0000−0.5∗(C+C1)∗dt00⎫⎭⎬⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪ Pδ=Fδ∗Pδ∗FTδ

gyro noise density： σgc accelerometer noise density： σac gyro drift noise density： σgwc accelerometer drift noise density： σawc

σ2dα=dt∗σgc∗σgc σ2v=dt∗σac∗σac σ2p=0.5∗dt∗dt∗σ2v σ2bg=dt∗σgwc∗σgwc σ2ba=dt∗σawc∗σawc

Pδ=Pδ+⎧⎩⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪σ2p∗I(3,3)00000σ2dα∗I(3,3)00000σ2v∗I(3,3)00000σ2bg∗I(3,3)00000σ2ba∗I(3,3)⎫⎭⎬⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪

end covariance propagation

Δq=Δq1 ∫C=∫′C ∫a=∫′a Mcross=Mcorss1 dvdbg=dvdbg′

end for

输出系统状态量更新

输入重力加速度参数： g gW=g∗(0,0,1)T t{TWS}=r0+v∗Δt+CWS0∗∬a−0.5∗gW∗Δt∗Δt q{TWS}=qWS0∗Δq v=v+CWS0∗∫a−gW∗Δt

输出的雅各比矩阵

J=I(15,15) J(0:2,3:5)=−[CWS0∗∬a]× J(0:2,6:8)=I(3,3)∗Δt J(0:2,9:11)=CWS0∗dpdbg J(0:2,12:14)=−CWS0∗∬C J(3:5,9:11)=−CWS0∗dαdbg J(6:8,3:5)=−[CWS0∗∫a]× J(6:8,9:11)=CWS0∗dvdbg J(6:8,12:14)=−CWS0∗∫C

J=⎧⎩⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪00000−[CWS0∗∬a]×0−[CWS0∗∫a]×00I(3,3)∗Δt0000CWS0∗dpdbg−CWS0∗dαdbgCWS0∗dvdbg00−CWS0∗∬C0−CWS0∗∫C00⎫⎭⎬⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪

输出方差矩阵

T=I(15,15)

T=⎧⎩⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪CWS000000CWS000000CWS0000000000000⎫⎭⎬⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪

P=T∗Pδ∗TT

版权声明：本文为博主原创文章，未经博主允许不得转载。