OKVIS 中的 propagation 公式版

来源：互联网发布：华康少女字体mac 编辑：程序博客网时间：2024/06/13 05:23

初值赋值

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 accumulatrionEigen::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 - [\int a * d t + 0.25 * (C + C 1) * S a * d t * d t] \times 0 - [0.5 * (C + C 1) * S a * d t] \times 00 I (3, 3) * d t 0000 d t * d v d b g + 0.25 * d t * d t * (C * [S a] \times * M c r o s s + C 1 * [S a] \times * M c r o s s 1) - d t * C 1 0.5 * d t * (C * [a S] \times * M c r o s s + C 1 * [a S] \times * M c r o s s 1) 00 00 - 0.5 * (C + C 1) * d t 00 ⎫ ⎭ ⎬ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪

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 δ + ⎧ ⎩ ⎨ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ σ 2 p * I (3, 3) 0000 0 σ 2 d α * I (3, 3) 000 00 σ 2 v * I (3, 3) 00 000 σ 2 b g * I (3, 3) 0 0000 σ 2 b a * 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 - [C W S 0 * \iint a] \times 0 - [C W S 0 * \int a] \times 00 I (3, 3) * Δ t 0000 C W S 0 * d p d b g - C W S 0 * d α d b g C W S 0 * d v d b g 00 - C W S 0 * \iint C 0 - C W S 0 * \int C 00 ⎫ ⎭ ⎬ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪

输出方差矩阵

方差 P 是从通过参数传进来，这里并没有清掉，说明是不断传播的
T=I(15,15)

T = ⎧ ⎩ ⎨ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ C W S 0 0000 0 C W S 0 000 00 C W S 0 00 0000000000 ⎫ ⎭ ⎬ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪ ⎪

P=T∗Pδ∗TT

1 0