OKVIS 中的 propagation 公式版

初值賦值

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

版權聲明：本文爲博主原創文章，未經博主允許不得轉載。