网站建设目标怎么看,稳稳在哪个网站做的消防直播,二手域名购买已备案,工业设计师年薪60万MSCKF3讲#xff1a;后端理论推导#xff08;上#xff09; 文章目录 MSCKF3讲#xff1a;后端理论推导#xff08;上#xff09;1 MSCKF中的状态变量① IMU状态:② cam0状态#xff1a;③ IMU和cam0间状态关系 2 微分方程递推#xff08;数值解#xff09;3 IMU状态预…MSCKF3讲后端理论推导上 文章目录 MSCKF3讲后端理论推导上1 MSCKF中的状态变量① IMU状态:② cam0状态③ IMU和cam0间状态关系 2 微分方程递推数值解3 IMU状态预测3.1 连续时间下**IMU状态**运动学方程微分方程① 状态理想值② 状态测量值 3.2 离散时间下**IMU状态**运动学方程差分方程3.2.1 预测姿态 G I q T _G^Iq^T GIqT3.2.2 预测速度 G v I T ^Gv_I^T GvIT和位置 G p I T ^Gp_I^T GpIT 4 IMU误差状态预测4.1 连续时间下 误差状态4.1.1 推导过程① 旋转② 速度③ 平移④ 偏置 4.1.2 结论 4.2 离散时间下误差状态4.2.1 均值4.2.2 协方差矩阵 4.3 状态增广4.3.1 什么是状态增广1 雅可比推导2 扩展误差状态协方差 4.3.2 为什么要状态增广 5 状态更新5.1 理论分析5.2 视觉观测雅可比矩阵H推导5.3 视觉观测雅可比的后续处理5.3.1 左零空间投影5.3.2 QR分解降低计算量 5.4 更新 6 左零空间投影6.1 为什么要进行左零空间投影6.2 如何计算零空间矩阵V 写在前面这篇论文中C表示旋转矩阵或者把q变为旋转矩阵的意思。 1 MSCKF中的状态变量 下面G表示世界系I表示IMU坐标系作为body系cam0表示左目相机系。关于坐标系表示的申明下面这种表达方式 G I R ^I_G{R} GIR等价于 R I G {R}_{IG} RIG是一种常见的转换关系。
① IMU状态:
(43333)×1 16×1 X I [ G I q ˉ T b g T G v I T b a T G p I T ] T \mathbf{X}_\mathrm{I}\begin{bmatrix}^I_G\bar{q}^T\mathbf{b}_g^T^G\mathbf{v}_I^T\mathbf{b}_a^T^G\mathbf{p}_I^T\end{bmatrix}^T XI[GIqˉTbgTGvITbaTGpIT]T G I q ˉ T ^I_G\bar{q}^T GIqˉT表示 the rotation from frame {G} to frame {I}描述世界系到惯性系的旋转用单位四元数表示4维。因为是JPL局部坐标系所以是 q I G q_{IG} qIG而不是 q G I q_{GI} qGI G p I T ^G\mathbf{p}_I^T GpIT和 G v I T ^G\mathbf{v}_I^T GvIT表示位置和速度即平移量the IMU position and velocity with respect to {G},在世界系下表示。 b g T \mathbf{b}_g^T bgT和 b a T \mathbf{b}_a^T baT分别表示the biases affecting the gyroscope and accelerometer measurements即陀螺仪和加速度计的偏差。
② cam0状态 利用滑动窗口来记录的不只是一帧的相机姿态。at any time instant the EKF state vector comprises (i) the evolving IMU state (ii)a history of up to N max past poses of the camera X C [ T m T ⋯ T n T ] T \mathbf{X}_\mathrm{C}\begin{bmatrix}\mathbf{T}_m^T\cdots\mathbf{T}_n^T\end{bmatrix}^T XC[TmT⋯TnT]T 注意相机的每个T都是由 C q G ^C{q}_G CqG和 G p C a m 0 ^G\mathbf{p}_{Cam0} GpCam0表示C也是cam0简单表示(43)×1×N
③ IMU和cam0间状态关系
(43)×1 C I q ⊤ I p C ⊤ _C^I\mathbf{q}^\top\quad{}^I\mathbf{p}_C^\top CIq⊤IpC⊤
2 微分方程递推数值解 微分方程 { y ′ ( x ) f ( x , y ) , x ∈ I ( a , b ) y ( x 0 ) y 0 \left.\left\{\begin{array}{l}y^{\prime}(x)f(x,y),\quad x\in I(a,b)\\y\left(x_0\right)y_0\end{array}\right.\right. {y′(x)f(x,y),x∈I(a,b)y(x0)y0 拉格朗日中值定理 y ( x n 1 ) − y ( x n ) h y ′ ( x θ h ) \frac{y\left(x_{n1}\right)-y\left(x_{n}\right)}hy^{\prime}(x\theta h) hy(xn1)−y(xn)y′(xθh) 欧拉法本质一阶泰勒线性近似求导 y ( x i Δ x ) y ( x i ) y ′ ( x i ) Δ x y ′ ′ ( x i ) 2 ! Δ x 2 ⋯ y ( n ) ( x i ) n ! Δ x n O ( Δ x n 1 ) y\left(x_i\Delta x\right)y\left(x_i\right)y^{\prime}\left(x_i\right)\Delta x\frac{y^{\prime\prime}\left(x_i\right)}{2!}\Delta x^2\cdots\frac{y^{\left(n\right)}\left(x_i\right)}{n!}\Delta x^nO\left(\Delta x^{n1}\right) y(xiΔx)y(xi)y′(xi)Δx2!y′′(xi)Δx2⋯n!y(n)(xi)ΔxnO(Δxn1) y ( x i Δ x ) − y ( x i ) Δ x O ( Δ x ) f ( x i , y ( x i ) ) \frac{y\left(x_{i}\Delta x\right)-y\left(x_{i}\right)}{\Delta x}O(\Delta x)f\left(x_{i},y\left(x_{i}\right)\right) Δxy(xiΔx)−y(xi)O(Δx)f(xi,y(xi)) { y i 1 y i ℏ f ( x i , y i ) , i 0 , 1 , ⋯ n − 1 y 0 y ( x 0 ) \left.\left\{\begin{array}{l}y_{i1}y_i\hbar f\left(x_i,y_i\right),\quad i0,1,\cdots n-1\\y_0y\left(x_0\right)\end{array}\right.\right. {yi1yiℏf(xi,yi),i0,1,⋯n−1y0y(x0) 四阶龙格库塔法 { y n 1 y n h 6 ( k 1 2 k 2 2 k 3 k 4 ) k 1 f ( x n , y n ) k 2 f ( x n h 2 , y n h 2 k 1 ) k 3 f ( x n ℏ 2 , y n ℏ 2 k 2 ) k 4 f ( x n h , y n h k 3 ) \left.\left\{\begin{array}{l}y_{n1}y_n\frac h6\left(k_12\boldsymbol{k}_22k_3k_4\right)\\k_1f\left(x_n,y_n\right)\\k_2f\left(x_n\frac h2,y_n\frac h2k_1\right)\\k_3f\left(x_n\frac\hbar2,y_n\frac\hbar2\boldsymbol{k}_2\right)\\\boldsymbol{k}_4\boldsymbol{f}\left(\boldsymbol{x}_n\boldsymbol{h},\boldsymbol{y}_n\boldsymbol{h}\boldsymbol{k}_3\right)\end{array}\right.\right. ⎩ ⎨ ⎧yn1yn6h(k12k22k3k4)k1f(xn,yn)k2f(xn2h,yn2hk1)k3f(xn2ℏ,yn2ℏk2)k4f(xnh,ynhk3)
3 IMU状态预测 预测是对状态估计量state进行迭代预测而不是误差状态向量Error State用来更新但协方差是根据误差状态的运动模型进行更新。 每一帧的IMU观测数据需要对 X I X_I XI中的姿态 G I q T _G^Iq^T GIqT、速度 G v I T ^Gv_I^T GvIT和位置 G p I T ^Gp_I^T GpIT进行预测偏差 b a , b g b_a,b_g ba,bg保持不变。 b g ( t Δ t ) b g ( t ) , b a ( t Δ t ) b a ( t ) , g ( t Δ t ) g ( t ) . \begin{aligned}\boldsymbol b_g(t\Delta t)\boldsymbol{b}_g(t),\\\boldsymbol{b}_a(t\Delta t)\boldsymbol{b}_a(t),\\\boldsymbol{g}(t\Delta t)\boldsymbol{g}(t).\end{aligned} bg(tΔt)ba(tΔt)g(tΔt)bg(t),ba(t),g(t). 这里相直接给出了离散时间下的IMU偏差的运动学方程没有给出从连续时间到离散的推导实际上因为偏置的导数是一个高斯过程其均值是0所以离散条件下偏置保持不变。
3.1 连续时间下IMU状态运动学方程微分方程 这部分参考论文A Multi-State Constraint Kalman Filterfor Vision-aided Inertial Navigation之3.1节
① 状态理想值 注意下面式子描述了IMU状态真值的运动学变化包括了均值和协方差 G I q ˉ ˙ ( t ) 1 2 Ω ( ω ( t ) ) G I q ˉ ( t ) , b ˙ g ( t ) n w g ( t ) v ˙ I ( t ) G a ( t ) , b ˙ a ( t ) n w a ( t ) , p ˙ I ( t ) G v I ( t ) \begin{aligned} {^{I}_{G}\dot{\bar{q}}(t)}\frac{1}{2}\boldsymbol{\Omega}\big(\boldsymbol{\omega}(t)\big)_{G}^{I}\bar{q}(t),\dot{\mathbf{b}}_{g}(t)\mathbf{n}_{wg}(t)\\\dot{\mathbf{v}}_{I}(t){}^{G}\mathbf{a}(t),\dot{\mathbf{b}}_{a}(t)\mathbf{n}_{wa}(t),\dot{\mathbf{p}}_{I}(t){}^{G}\mathbf{v}_{I}(t)\end{aligned} GIqˉ˙(t)v˙I(t)21Ω(ω(t))GIqˉ(t),Ga(t),b˙g(t)b˙a(t)nwg(t)nwa(t),p˙I(t)GvI(t) 其中矩阵 Ω ( ω ) {\Omega}(\boldsymbol{\omega}) Ω(ω) Ω ( ω ) [ − ⌊ ω × ⌋ ω − ω T 0 ] , ⌊ ω × ⌋ [ 0 − ω z ω y ω z 0 − ω x − ω y ω x 0 ] \boldsymbol{\Omega}(\boldsymbol{\omega})\begin{bmatrix}-\lfloor\boldsymbol{\omega}\times\rfloor\boldsymbol{\omega}\\-\boldsymbol{\omega}^T0\end{bmatrix},\quad\lfloor\boldsymbol{\omega}\times\rfloor\begin{bmatrix}0-\omega_z\omega_y\\\omega_z0-\omega_x\\-\omega_y\omega_x0\end{bmatrix} Ω(ω)[−⌊ω×⌋−ωTω0],⌊ω×⌋ 0ωz−ωy−ωz0ωxωy−ωx0 角速度 ω m {\omega}_m ωm和加速度 a m {a}_m am的测量值 ω m ω C ( G I q ˉ ) ω G b g n g a m C ( G I q ˉ ) ( G a − G g 2 ⌊ ω G × ⌋ G v I ⌊ ω G × ⌋ 2 p I ) b a n a \begin{aligned}\mathbf{\omega}_m\mathbf{\omega}\mathbf{C}(_G^I\bar{q})\mathbf{\omega}_G\mathbf{b}_g\mathbf{n}_g\\\mathbf{a}_m\mathbf{C}(_G^I\bar{q})(^G\mathbf{a}-^G\mathbf{g}2\lfloor\omega_G\times\rfloor^G\mathbf{v}_I\lfloor\omega_G\times\rfloor^2\mathbf{p}_I)\mathbf{b}_a\mathbf{n}_a\end{aligned} ωmωC(GIqˉ)ωGbgngamC(GIqˉ)(Ga−Gg2⌊ωG×⌋GvI⌊ωG×⌋2pI)bana 上面 ω G \mathbf{\omega}_G ωG论文中指the IMU measurements incorporate the effects of the planet’s rotation受到行星自转的影响但是大部分实现中都没有考虑这个 一般来说如果把IMU放在车体中央可以省略其它元素对加速度干扰即加速度 a m {a}_m am可以表示为下式 a m C ( G I q ˉ ) ( G a − G g ) b a n a \begin{aligned}\mathbf{a}_m\mathbf{C}(_G^I\bar{q})(^G\mathbf{a}-^G\mathbf{g})\mathbf{b}_a\mathbf{n}_a\end{aligned} amC(GIqˉ)(Ga−Gg)bana
② 状态测量值 对上面公式取均值之后就可以得到IMU状态测量值运动学公式注意高斯过程和高斯噪声的均值皆为0所以下面偏置导数和噪声均值皆可置零。下面 C q ^ C ( G I q ⃗ ^ ) , a ^ a m − b ^ a a n d ω ^ ω m − b ^ g − C q ^ ω G . \mathbf{C}_{\hat{q}}\mathbf{C}(_{G}^{I}\hat{\vec{q}}),\mathbf{\hat{a}}\mathbf{a}_{m}-\hat{\mathbf{b}}_{a}\mathrm{and}\boldsymbol{\hat{\omega}}\boldsymbol{\omega}_{m}-\hat{\mathbf{b}}_{g}-\mathbf{C}_{\hat{q}}\boldsymbol{\omega}_{G}. Cq^C(GIq ^),a^am−b^aandω^ωm−b^g−Cq^ωG. G I q ˉ ^ ˙ 1 2 Ω ( ω ^ ) G I q ˉ ^ , b ^ g 0 3 × 1 , G v ^ ˙ I C q ^ T a ^ − 2 ⌊ ω G × ⌋ G v ^ I − ⌊ ω G × ⌋ 2 G p ^ I G g b ^ ˙ a 0 3 × 1 , G p ^ ˙ I G v ^ I \begin{gathered} {}_{G}^{I}\dot{\hat{\bar{q}}}\frac{1}{2}\boldsymbol{\Omega}(\hat{\boldsymbol{\omega}})_{G}^{I}\hat{\bar{q}},\quad\hat{\mathbf{b}}_{g}\mathbf{0}_{3\times1}, \\ {}^{G}\dot{\hat{\mathbf{v}}}_{I}\mathbf{C}_{\hat{q}}^{T}\hat{\mathbf{a}}-2\lfloor\boldsymbol{\omega}_{G}\times\rfloor{}^{G}\hat{\mathbf{v}}_{I}-\lfloor\boldsymbol{\omega}_{G}\times\rfloor{}^{2G}\hat{\mathbf{p}}_{I}{}^{G}\mathbf{g} \\ \dot{\hat{\mathbf{b}}}_{a}\mathbf{0}_{3\times1},\quad G\dot{\hat{\mathbf{p}}}_{I}G\hat{\mathbf{v}}_{I} \end{gathered} GIqˉ^˙21Ω(ω^)GIqˉ^,b^g03×1,Gv^˙ICq^Ta^−2⌊ωG×⌋Gv^I−⌊ωG×⌋2Gp^IGgb^˙a03×1,Gp^˙IGv^I 代码实现并没有考虑行星自转以及加速度相关影响本质上就是最简单的IMU运动模型对应论文Robust Stereo Visual Inertial Odometry for Fast Autonomous Flight公式1 3.2 离散时间下IMU状态运动学方程差分方程 从连续到离散不论是对于状态方程还是误差状态方程都很简单比如状态方程中如果只是使用欧拉法去进行积分那么速度平移等公式就像是高中物理一般 这个过程也是卡尔曼滤波的第一步预测均值Propagation Equations
3.2.1 预测姿态 G I q T _G^Iq^T GIqT 论文Indirect Kalman Filter for 3D Attitude Estimation1.6节给出了具体推导0阶和1阶的区别就是在于在单位时间内角速度是否变化。0阶不变一直是 ω ( t k ) {\omega}(t_k) ω(tk)1阶用两个时间点取了均值 ω ˉ ω ( t k 1 ) ω ( t k ) 2 \bar{\boldsymbol{\omega}}\frac{\boldsymbol{\omega}(t_{k1})\boldsymbol{\omega}(t_k)}2 ωˉ2ω(tk1)ω(tk) MSCKF中使用的是**0阶积分(假设角速度在单位时间内恒定不变)**其中 ω ^ ω m − b ^ g \hat{\omega}\omega_m-\hat{b}_g ω^ωm−b^g ∣ ω ^ ∣ 1 0 − 5 时: G I q ^ ( t Δ t ) ( cos ( ∣ ω ^ ∣ 2 Δ t ) ⋅ I 4 × 4 1 ∣ ω ^ ∣ sin ( ∣ ω ^ ∣ 2 Δ t ) ⋅ Ω ( ω ^ ) ) G I q ^ ( t ) ∣ ω ^ ∣ ≤ 1 0 − 5 时: G I q ^ ( t Δ t ) ( I 4 × 4 − Δ t 2 Ω ( ω ^ ) ) G I q ^ ( t ) \begin{aligned}|\hat{\omega}|10^{-5}\text{ 时: }^I_G\hat{q}(t\Delta t)\left(\cos\left(\frac{|\hat{\omega}|}2\Delta t\right)\cdot I_{4\times4}\frac1{|\hat{\omega}|}\sin\left(\frac{|\hat{\omega}|}2\Delta t\right)\cdot\Omega(\hat{\omega})\right) {^I_G\hat{q}}(t)\\|\hat{\omega}|\leq10^{-5}\text{ 时: }^I_G\hat{q}(t\Delta t)\left(I_{4\times4}-\frac{\Delta t}2\Omega(\hat{\omega})\right){^I_G\hat{q}(t)}\end{aligned} ∣ω^∣10−5 时: GIq^(tΔt)(cos(2∣ω^∣Δt)⋅I4×4∣ω^∣1sin(2∣ω^∣Δt)⋅Ω(ω^))GIq^(t)∣ω^∣≤10−5 时: GIq^(tΔt)(I4×4−2ΔtΩ(ω^))GIq^(t) 姿态 G I q T _G^Iq^T GIqT的预测利用了角速度其中 ω ^ ω m − b ^ g \hat{\omega}\omega_m-\hat{b}_g ω^ωm−b^g。下标m表示测量值测量值减去一个偏差表示理想值。这里和高博新书表示略有不同一般都是 ω m ω ^ b ^ g {\omega}_m\hat\omega\hat{b}_g ωmω^b^g即测量 理想值 偏差。
3.2.2 预测速度 G v I T ^Gv_I^T GvIT和位置 G p I T ^Gp_I^T GpIT 要注意的是一个点在不同坐标系下的速度矢量并不相同不是一个矢量在不同坐标系中的表达参考高博新书。我们一般说的速度都是指机器人本身在世界系下的速度其相对于自身坐标系速度一直都是0。 注意这里的速度和位置都是在世界系下的矢量加速度是在IMU系下的量所以要变换到世界系下还有 g ( 0 , 0 , − 9.8 ) g (0,0,-9.8) g(0,0,−9.8)。下标m值测量值。 p ^ ˙ ( t ) v ^ ( t ) v ^ ˙ ( t ) q ^ ( t ) a ^ g a ^ a m − b ^ a \begin{gathered}\dot{\hat{\mathrm{p}}}(\mathrm{t})\hat{\mathrm{v}}(\mathrm{t})\\\dot{\hat{\mathrm{v}}}(\mathrm{t})\hat{\mathrm{q}}(\mathrm{t})\hat{a}\mathrm{g}\\\hat{a}a_m-\hat{b}_a\end{gathered} p^˙(t)v^(t)v^˙(t)q^(t)a^ga^am−b^a 姿态的预测采用了比较简单的欧拉法即认为角速度在单位时间内恒定不变。对于速度和位置的预测采用了更加平滑的龙格库塔法 v ^ ( t Δ t ) v ^ ( t ) Δ t 6 ( k v 1 2 k v 2 2 k v 3 k v 4 ) k v 1 R ^ ( t ) a ^ g k v 2 R ^ ( t Δ t / 2 ) a ^ g k v 3 R ^ ( t Δ t / 2 ) a ^ g k v 4 R ^ ( t Δ t ) a ^ g p ^ ( t Δ t ) p ^ ( t ) Δ t 6 ( k p 1 2 k p 2 2 k p 3 k p 4 ) k p 1 v ^ ( t ) k p 2 v ^ ( t ) k v 1 Δ t / 2 k p 3 v ^ ( t ) k v 2 Δ t / 2 k p 4 v ^ ( t ) k v 3 Δ t \begin{aligned} \hat{v}(t\Delta t)\hat{v}(t)\frac{\Delta t}6\left(k_{v_1}2k_{v_2}2k_{v_3}k_{v_4}\right) \\ k_{v_1}\hat{R}(t)\hat{a}g \\ k_{v_2}\hat{R}(t\Delta t/2)\hat{a}g \\ k_{v_3}\hat{R}(t\Delta t/2)\hat{a}g \\ k_{v_4}\hat{R}(t\Delta t)\hat{a}g \\ \hat{p}(t\Delta t)\hat{p}(t)\frac{\Delta t}6\left(k_{p_1}2k_{p_2}2k_{p_3}k_{p_4}\right) \\ k_{p_1}\hat{v}(t) \\ k_{p_2}\hat{v}(t)k_{v_1}\Delta t/2 \\ k_{p_3}\hat{v}(t)k_{v_2}\Delta t/2 \\ k_{p_4}\hat{v}(t)k_{v_3}\Delta t \end{aligned} v^(tΔt)v^(t)6Δt(kv12kv22kv3kv4)kv1R^(t)a^gkv2R^(tΔt/2)a^gkv3R^(tΔt/2)a^gkv4R^(tΔt)a^gp^(tΔt)p^(t)6Δt(kp12kp22kp3kp4)kp1v^(t)kp2v^(t)kv1Δt/2kp3v^(t)kv2Δt/2kp4v^(t)kv3Δt 速度 G v I T ^Gv_I^T GvIT和位置 G p I T ^Gp_I^T GpIT的预测利用了加速度
4 IMU误差状态预测 IMU更新时用龙格库塔法得到了新的IMU状态 我们现在还要递推什么递推IMU误差协方差矩阵。实际更新的算法使用了ESKF维护的是误差状态因此要推出新一时刻的误差状态协方差与上⼀时刻误差状态协方差及传感器误差协方差的关系。 下面结论参考论文A Multi-State Constraint Kalman Filter for Vision-aided Inertial Navigation。 误差怎么表示 理想数值 估计数值 误差
4.1 连续时间下 误差状态
4.1.1 推导过程
① 旋转 四元数的推导比较麻烦参考Indirect Kalman Filter for 3D Attitude Estimation 2.4节 Continuous Time Error State Equations
② 速度
区分清楚估计值就是利用测量值估计的一个数 ③ 平移 平移的导数即速度 G δ p ˙ I G δ v I ^G\delta\mathbf{\dot p}_I ^G\delta\mathbf{v}_I Gδp˙IGδvI
④ 偏置 IMU噪声模型中说的很清楚了偏差的导数是高斯过程0均值。 δ b ˙ n w \delta\dot{\mathbf{b}}\mathbf{n_w} δb˙nw
4.1.2 结论 写成矩阵形式 δ x ˙ F c ⋅ δ x G c ⋅ n \dot{\delta\mathbf{x}}\mathbf{F}_c\cdot\delta\mathbf{x}\mathbf{G}_c\cdot\mathbf{n} δx˙Fc⋅δxGc⋅n 变量 δ x ( G I δ θ ⊤ δ b g ⊤ G δ v I ⊤ δ b a ⊤ G δ p I ⊤ C I δ θ ⊤ I δ p C ⊤ ) ⊤ \left.\delta\mathbf{x}\left(\begin{array}{cccccc}^I_G\delta\boldsymbol{\theta}^\top\delta\mathbf{b}_g^\top^G\delta\mathbf{v}_I^\top\delta\mathbf{b}_a^\top^G\delta\mathbf{p}_I^\top{}^I_C\delta\boldsymbol{\theta}^\top{}^I\delta\mathbf{p}_C^\top\end{array}\right.\right)^\top δx(GIδθ⊤δbg⊤GδvI⊤δba⊤GδpI⊤CIδθ⊤IδpC⊤)⊤ n ( n g n w g n a n w a ) ⊤ \left.\mathbf{n}\left(\begin{array}{ccc}\mathbf{n_g}\mathbf{n_{wg}}\mathbf{n_a}\mathbf{n_{wa}}\end{array}\right.\right)^\top n(ngnwgnanwa)⊤ 矩阵F注意点 ① 速度即第三行忽略了相关因素影响3.1中①中描述了 ② 外参相机与IMU的转换这个是不变的所以第6、7行都是0 ③ 这里只是5个变量S-MSCKF论文以及代码中都还有外参两个自变量所以F实际是21*21矩阵。7个变量每个都是3×3 ④ 参见Robust Stereo Visual Inertial Odometry for Fast Autonomous Flight公式2 F ( − ⌊ ω ^ × ⌋ − I 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 − C ( C I q ^ ) ⊤ ⌊ a ^ × ⌋ 0 3 × 3 0 3 × 3 − C ( G I q ^ ) ⊤ 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 I 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 ) \mathbf{F}\begin{pmatrix}-\lfloor\hat{\boldsymbol{\omega}}_{\times}\rfloor-\mathbf{I}_{3}\mathbf{0}_{3\times3}\mathbf{0}_{3\times3}\mathbf{0}_{3\times3}\\\mathbf{0}_{3\times3}\mathbf{0}_{3\times3}\mathbf{0}_{3\times3}\mathbf{0}_{3\times3}\mathbf{0}_{3\times3}\\-C\left( ^{I}_{C}\hat{\mathbf{q}}\right)^\top\left\lfloor\hat{\mathbf{a}}_{\times}\right\rfloor\mathbf{0}_{3\times3}\mathbf{0}_{3\times3}-C\left( ^{I}_{G}{\hat{\mathbf{q}}}\right)^\top\mathbf{0}_{3\times3}\\\mathbf{0}_{3\times3}\mathbf{0}_{3\times3}\mathbf{0}_{3\times3}\mathbf{0}_{3\times3}\mathbf{0}_{3\times3}\\\mathbf{0}_{3\times3}\mathbf{0}_{3\times3}\mathbf{I}_{3}\mathbf{0}_{3\times3}\mathbf{0}_{3\times3}\\\mathbf{0}_{3\times3}\mathbf{0}_{3\times3}\mathbf{0}_{3\times3}\mathbf{0}_{3\times3}\mathbf{0}_{3\times3}\\\mathbf{0}_{3\times3}\mathbf{0}_{3\times3}\mathbf{0}_{3\times3}\mathbf{0}_{3\times3}\mathbf{0}_{3\times3} \end{pmatrix} F −⌊ω^×⌋03×3−C(CIq^)⊤⌊a^×⌋03×303×303×303×3−I303×303×303×303×303×303×303×303×303×303×3I303×303×303×303×3−C(GIq^)⊤03×303×303×303×303×303×303×303×303×303×303×3
G ( − I 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 I 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 − C ( G I q ^ ) ⊤ 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 I 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 ) \left.\mathbf{G}\left(\begin{array}{cccc}-\mathbf{I}_3\mathbf{0}_{3\times3}\mathbf{0}_{3\times3}\mathbf{0}_{3\times3}\\\mathbf{0}_{3\times3}\mathbf{I}_3\mathbf{0}_{3\times3}\mathbf{0}_{3\times3}\\\mathbf{0}_{3\times3}\mathbf{0}_{3\times3}-C\left(^{I}_{G}\hat{\mathbf{q}}\right)^\top\mathbf{0}_{3\times3}\\\mathbf{0}_{3\times3}\mathbf{0}_{3\times3}\mathbf{0}_{3\times3}\mathbf{0}_{3\times3}\\\mathbf{0}_{3\times3}\mathbf{0}_{3\times3}\mathbf{0}_{3\times3}\mathbf{I}_3\\\mathbf{0}_{3\times3}\mathbf{0}_{3\times3}\mathbf{0}_{3\times3}\mathbf{0}_{3\times3}\\\mathbf{0}_{3\times3}\mathbf{0}_{3\times3}\mathbf{0}_{3\times3}\mathbf{0}_{3\times3}\end{array}\right.\right) G −I303×303×303×303×303×303×303×3I303×303×303×303×303×303×303×3−C(GIq^)⊤03×303×303×303×303×303×303×303×3I303×303×3 补充关于速度考虑对于加速度a的各种影响则F是下式这里省略了6、7行的0矩阵矩阵G是相同的。A Multi-State Constraint Kalman Filterfor Vision-aided Inertial Navigation公式10 F [ − ⌊ ω ^ × ⌋ − I 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 − C q ^ T ⌊ a ^ × ⌋ 0 3 × 3 − 2 ⌊ ω G × ⌋ − C q ^ T − ⌊ ω G × ⌋ 2 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 I 3 0 3 × 3 0 3 × 3 ] \left.\mathbf{F}\left[\begin{array}{ccccc}-\lfloor\hat{\boldsymbol{\omega}}\times\rfloor-\mathbf{I}_3\mathbf{0}_{3\times3}\mathbf{0}_{3\times3}\mathbf{0}_{3\times3}\\\mathbf{0}_{3\times3}\mathbf{0}_{3\times3}\mathbf{0}_{3\times3}\mathbf{0}_{3\times3}\mathbf{0}_{3\times3}\\-\mathbf{C}_{\hat{q}}^T\lfloor\hat{\mathbf{a}}\times\rfloor\mathbf{0}_{3\times3}-2\lfloor\omega_G\times\rfloor-\mathbf{C}_{\hat{q}}^T-\lfloor\omega_G\times\rfloor^2\\\mathbf{0}_{3\times3}\mathbf{0}_{3\times3}\mathbf{0}_{3\times3}\mathbf{0}_{3\times3}\mathbf{0}_{3\times3}\\\mathbf{0}_{3\times3}\mathbf{0}_{3\times3}\mathbf{I}_3\mathbf{0}_{3\times3}\mathbf{0}_{3\times3}\end{array}\right.\right] F −⌊ω^×⌋03×3−Cq^T⌊a^×⌋03×303×3−I303×303×303×303×303×303×3−2⌊ωG×⌋03×3I303×303×3−Cq^T03×303×303×303×3−⌊ωG×⌋203×303×3 注意我们并不会直接使用F和G去预测等等而是在求解离散时间下协方差时利用。 4.2 离散时间下误差状态 参考Indirect Kalman Filter for 3D Attitude Estimation 2.5节 Discrete Time Error State Equations 对一个线性系统做离散化是由固定的通解的见线性系统理论笔记如下 δ x ( t Δ t ) Φ ( t Δ t , t ) δ x t ∫ t t Δ t Φ ( t Δ t , τ ) G c ( τ ) n ( τ ) d τ \delta\mathbf{x}(t\Delta t)\boldsymbol{\Phi}\left(t\Delta t,t\right)\delta\mathbf{x}_t\int_t^{t\Delta t}\boldsymbol{\Phi}(t\Delta t,\tau)\mathbf{G}_\mathbf{c}(\tau)\mathbf{n}(\tau)d\tau δx(tΔt)Φ(tΔt,t)δxt∫ttΔtΦ(tΔt,τ)Gc(τ)n(τ)dτ 把一个连续系统离散化在线性系统理论中讲过系统矩阵 F F F变为了 状态转移矩阵$ \Phi(t\Delta t,t) exp(Ft)$ Φ ( t Δ t , t ) exp ( F c Δ t ) I 6 × 6 F c Δ t 1 2 ! F c 2 Δ t 2 … \begin{aligned} \Phi(t\Delta t,t) \exp\left(\mathbf{F}_{c}\Delta t\right) \\ \mathbf{I}_{6\times6}\mathbf{F}_{c}\Delta t\frac{1}{2!}\mathbf{F}_{c}^{2}\Delta t^{2}\ldots \end{aligned} Φ(tΔt,t)exp(FcΔt)I6×6FcΔt2!1Fc2Δt2… 其中幂指数对应代码 F c [ − ⌊ ω ^ × ⌋ − I 3 × 3 0 3 × 3 0 3 × 3 ] , F c 2 [ ⌊ ω ^ × ⌋ 2 ⌊ ω ^ × ⌋ 0 3 × 3 0 3 × 3 ] F c 3 [ − ⌊ ω ^ × ⌋ 3 − ⌊ ω ^ × ⌋ 2 0 3 × 3 0 3 × 3 ] , F c 4 [ ⌊ ω ^ × ⌋ 4 ⌊ ω ^ × ⌋ 3 0 3 × 3 0 3 × 3 ] \begin{aligned}\mathbf{F}_c\begin{bmatrix}-\lfloor\hat{\boldsymbol{\omega}}\times\rfloor-\mathbf{I}_{3\times3}\\\mathbf{0}_{3\times3}\mathbf{0}_{3\times3}\end{bmatrix}\quad,\quad\mathbf{F}_c^2\begin{bmatrix}\lfloor\hat{\boldsymbol{\omega}}\times\rfloor^2\lfloor\hat{\boldsymbol{\omega}}\times\rfloor\\\mathbf{0}_{3\times3}\mathbf{0}_{3\times3}\end{bmatrix}\\\mathbf{F}_c^3\begin{bmatrix}-\lfloor\hat{\boldsymbol{\omega}}\times\rfloor^3-\lfloor\hat{\boldsymbol{\omega}}\times\rfloor^2\\\mathbf{0}_{3\times3}\mathbf{0}_{3\times3}\end{bmatrix}\quad,\quad\mathbf{F}_c^4\begin{bmatrix}\lfloor\hat{\boldsymbol{\omega}}\times\rfloor^4\lfloor\hat{\boldsymbol{\omega}}\times\rfloor^3\\\mathbf{0}_{3\times3}\mathbf{0}_{3\times3}\end{bmatrix}\end{aligned} Fc[−⌊ω^×⌋03×3−I3×303×3],Fc2[⌊ω^×⌋203×3⌊ω^×⌋03×3]Fc3[−⌊ω^×⌋303×3−⌊ω^×⌋203×3],Fc4[⌊ω^×⌋403×3⌊ω^×⌋303×3] 代入通解可得到 δ x t Δ t ( I 6 × 6 F c Δ t 1 2 ! F c 2 Δ t 2 … ) δ x t ∫ t t Δ t Φ ( t Δ t , τ ) G c ( τ ) n ( τ ) d τ \delta\mathbf{x}_{t\Delta t}(\mathbf{I}_{6\times6}\mathbf{F}_c\Delta t\frac1{2!}\mathbf{F}_c^2\Delta t^2\ldots)\delta\mathbf{x}_t\int_t^{t\Delta t}\mathbf{\Phi}(t\Delta t,\tau)\mathbf{G}_\mathbf{c}(\tau)\mathbf{n}(\tau)d\tau δxtΔt(I6×6FcΔt2!1Fc2Δt2…)δxt∫ttΔtΦ(tΔt,τ)Gc(τ)n(τ)dτ
4.2.1 均值 在MSCKF中误差状态的均值实际上没有被使用所以相关的论文里面也没有给出均值的推导不过把连续变为离散是一件较为容易的事情参考状态类似于下面形式高博新书给出的但具体肯定是有所差别的看是否利用高阶的积分来推导如龙格库塔法 δ p ( t Δ t ) δ p δ v Δ t , δ υ ( t Δ t ) δ v ( − R ( a ~ − b a ) ∧ δ θ − R δ b a δ g ) Δ t − η v , δ θ ( t Δ t ) E x p ( − ( ω ~ − b g ) Δ t ) δ θ − δ b g Δ t − η θ , δ b g ( t Δ t ) δ b g η b g , δ b a ( t Δ t ) δ b a η b a , \begin{aligned} \delta\boldsymbol{p}(t\Delta t) \delta\boldsymbol{p}\delta\boldsymbol{v}\Delta t, \\ \delta\boldsymbol{\upsilon}(t\Delta t) \delta\boldsymbol{v}(-\boldsymbol{R}(\tilde{\boldsymbol{a}}-\boldsymbol{b}_a)^{\wedge}\delta\boldsymbol{\theta}-\boldsymbol{R}\delta\boldsymbol{b}_a\delta\boldsymbol{g})\Delta t-\boldsymbol{\eta}_v, \\ \delta\boldsymbol{\theta}(t\Delta t) \mathrm{Exp}\left(-(\tilde{\boldsymbol{\omega}}-\boldsymbol{b}_g)\Delta t\right)\delta\boldsymbol{\theta}-\delta\boldsymbol{b}_g\Delta t-\boldsymbol{\eta}_\theta, \\ \delta\boldsymbol{b}_{g}(t\Delta t) \delta\boldsymbol{b}_g\boldsymbol{\eta}_{bg}, \\ \begin{aligned}\delta\boldsymbol{b}_{a}(t\Delta t)\end{aligned} \delta\boldsymbol{b}_{a}\boldsymbol{\eta}_{ba}, \end{aligned} δp(tΔt)δυ(tΔt)δθ(tΔt)δbg(tΔt)δba(tΔt)δpδvΔt,δv(−R(a~−ba)∧δθ−Rδbaδg)Δt−ηv,Exp(−(ω~−bg)Δt)δθ−δbgΔt−ηθ,δbgηbg,δbaηba,
4.2.2 协方差矩阵 写在前面其实这里的状态转移矩阵会经过可观测性约束进行一定的修改. 实际上是对离散化后的方程分两步计算一个是前面状态转移矩阵线性协方差计算再加上后面高斯噪声的协方差。两个都符合高斯分布所以协方差计算也符合。-----这里也说明虽然我们不用连续时间下的误差状态但是会利用这个F和G矩阵这也是为什么前面会求解。 P k 1 ∣ k Φ k P k ∣ k Φ k T Q d , k Φ k Φ ( t k 1 , t k ) exp ( ∫ t k t k 1 F ( τ ) d τ ) Q d , k ∫ t k t k 1 Φ ( t k 1 , τ ) G c ( τ ) Q c G c T ( τ ) Φ T ( t k 1 , τ ) d τ \begin{gathered}\mathbf{P}_{k1|k}\Phi_k\mathbf{P}_{k|k}\Phi_k^T\mathbf{Q}_{d,k} \\\Phi_k\Phi(t_{k1},t_k)\exp\left(\int_{t_k}^{t_{k1}}\mathbf{F}(\tau)d\tau\right)\\ \mathbf{Q}_{d,k}\int_{t_k}^{t_{k1}}\boldsymbol{\Phi}\left(t_{k1},\tau\right)\mathbf{G}_c(\tau)\mathbf{Q}_c\mathbf{G}_c^\mathrm{T}(\tau)\boldsymbol{\Phi}^\mathrm{T}\left(t_{k1},\tau\right)\mathrm{d}\tau\end{gathered}\\ Pk1∣kΦkPk∣kΦkTQd,kΦkΦ(tk1,tk)exp(∫tktk1F(τ)dτ)Qd,k∫tktk1Φ(tk1,τ)Gc(τ)QcGcT(τ)ΦT(tk1,τ)dτ Q c Q_c Qc是噪声带来的协方差。噪声是⾼斯白噪声且不同时刻是独立的所以每个变量至只与自身相关与其它无关也就是协方差矩阵非对角线元素全是0因此连续时间系统噪声协方差矩阵由 Q c E [ n ( t τ ) n T ( t ) ] [ N r 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 N w r 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 N a 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 N w a ] [ σ r c 2 ⋅ I 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 σ w c 2 ⋅ I 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 σ a c 2 ⋅ I 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 σ w a c 2 ⋅ I 3 × 3 ] \begin{gathered} \mathbf{Q}_c \left.E\left[\mathbf{n}(t\tau)\mathbf{n}^\mathrm{T}(t)\right]\left[\begin{array}{cccc}\mathbf{N}_\mathrm{r}\mathbf{0}_{3\times3}\mathbf{0}_{3\times3}\mathbf{0}_{3\times3}\\\mathbf{0}_{3\times3}\mathbf{N}_\mathrm{wr}\mathbf{0}_{3\times3}\mathbf{0}_{3\times3}\\\mathbf{0}_{3\times3}\mathbf{0}_{3\times3}\mathbf{N}_\mathrm{a}\mathbf{0}_{3\times3}\\\mathbf{0}_{3\times3}\mathbf{0}_{3\times3}\mathbf{0}_{3\times3}\mathbf{N}_\mathrm{wa}\end{array}\right.\right] \\ \left.\left[\begin{array}{cccc}\sigma_{r_c}^2\cdot\mathbf{I}_{3\times3}\mathbf{0}_{3\times3}\mathbf{0}_{3\times3}\mathbf{0}_{3\times3}\\\mathbf{0}_{3\times3}\sigma_{w_c}^2\cdot\mathbf{I}_{3\times3}\mathbf{0}_{3\times3}\mathbf{0}_{3\times3}\\\mathbf{0}_{3\times3}\mathbf{0}_{3\times3}\sigma_{a_c}^2\cdot\mathbf{I}_{3\times3}\mathbf{0}_{3\times3}\\\mathbf{0}_{3\times3}\mathbf{0}_{3\times3}\mathbf{0}_{3\times3}\sigma_{w_{ac}}^2\cdot\mathbf{I}_{3\times3}\end{array}\right.\right] \end{gathered} QcE[n(tτ)nT(t)] Nr03×303×303×303×3Nwr03×303×303×303×3Na03×303×303×303×3Nwa σrc2⋅I3×303×303×303×303×3σwc2⋅I3×303×303×303×303×3σac2⋅I3×303×303×303×303×3σwac2⋅I3×3
实际上的协方差矩阵还有相机的影响 当下一个 IMU即第 k1 个 IMU 来到后此时 k1 时刻的整体协方差矩阵可写为 P k 1 ∣ k ( P I I k 1 ∣ k Φ k P I C k ∣ k P I C k ∣ k ⊤ Φ k ⊤ P C C k ∣ k ) \left.\mathbf{P}_{k1|k}\left(\begin{matrix}\mathbf{P}_{II_{k1|k}}\mathbf{\Phi}_{k}\mathbf{P}_{IC_{k|k}}\\\mathbf{P}_{IC_{k|k}}^\top\mathbf{\Phi}_{k}^\top\mathbf{P}_{CC_{k|k}}\end{matrix}\right.\right) Pk1∣k(PIIk1∣kPICk∣k⊤Φk⊤ΦkPICk∣kPCCk∣k) 递推阶段关于相机的加持都是0也就是这个阶段关于相机状态部分是“不动”。但是公共部分是需要改变的因为IMU部分变了 P k 1 ∣ k ( P I I k 1 ∣ k Φ k P I C k ∣ k P I C k ∣ k ⊤ Φ k ⊤ P C C k ∣ k ) \left.\mathbf{P}_{k1|k}\left(\begin{matrix}\mathbf{P}_{II_{k1|k}}\mathbf{\Phi}_{k}\mathbf{P}_{IC_{k|k}}\\\mathbf{P}_{IC_{k|k}}^{\top}\mathbf{\Phi}_{k}^{\top}\mathbf{P}_{CC_{k|k}}\end{matrix}\right.\right) Pk1∣k(PIIk1∣kPICk∣k⊤Φk⊤ΦkPICk∣kPCCk∣k)
4.3 状态增广
4.3.1 什么是状态增广 其次搞明白什么是状态增广 状态增广cam新来了⼀帧状态跟协方差矩阵会有哪些变化 在没有图像进来时对IMU状态进行预测并计算系统误差状态协方差矩阵在有图像进来时根据相机与IMU的相对外参计算当前相机的位姿。然后将最新的相机状态加入到系统状态向量中去然后扩增误差状态协方差矩阵。
① 利用最新imu状态和外参计算当前相机位姿 G C q ^ I C q ^ ⊗ G I q ^ , G p ^ C G p ^ C C ( G I q ^ ) ⊤ I p ^ C \left._G^C\hat{\mathbf{q}}{}_I^C\hat{\mathbf{q}}\otimes{}_G^I\hat{\mathbf{q}},\quad{}^G\hat{\mathbf{p}}_C{}^G\hat{\mathbf{p}}_CC\left(\begin{matrix}^I_G\hat{\mathbf{q}}\end{matrix}\right.\right)^\top{}^I\hat{\mathbf{p}}_C GCq^ICq^⊗GIq^,Gp^CGp^CC(GIq^)⊤Ip^C
② 相机位姿分别对imu状态和外参的雅可比扩展误差状态协方差 P ( 21 6 ( N 1 ) ) × ( 21 6 ( N 1 ) ) [ I 21 6 N J 6 × ( 21 6 N ) ] P ( 21 6 N ) × ( 21 6 N ) [ I 21 6 N J 6 × ( 21 6 N ) ] T [ P P J T J P J P J T ] \begin{gathered} P^{(216(N1))\times(216(N1))} \left.\left[\begin{array}{c}I_{216N}\\J_{6\times(216N)}\end{array}\right.\right]P^{(216N)\times(216N)}\left[\begin{array}{c}I_{216N}\\J_{6\times(216N)}\end{array}\right]^T \\ \left.\left[\begin{array}{cc}PPJ^T\\JPJPJ^T\end{array}\right.\right] \end{gathered} P(216(N1))×(216(N1))[I216NJ6×(216N)]P(216N)×(216N)[I216NJ6×(216N)]T[PJPPJTJPJT]
1 雅可比推导 关于雅可比维度6*216N6N是指对之前的外参的雅可比全为0.行维表示 R c w R_{cw} Rcw和 t w c t_{wc} twc分别对 x ~ I ( G I θ ~ ⊤ b ~ g ⊤ G v ~ I ⊤ b ~ a ⊤ G p ~ I ⊤ I θ ~ C ⊤ I p ~ C ⊤ ) ⊤ \tilde{\mathbf{x}}_{I}\begin{pmatrix}^I_{G}\tilde{\boldsymbol{\theta}}^{\top}\tilde{\mathbf{b}}_{g}^{\top}^{G}\tilde{\mathbf{v}}_{I}^{\top}\tilde{\mathbf{b}}_{a}^{\top}^{G}\tilde{\mathbf{p}}_{I}^{\top}^{I}\tilde{\boldsymbol{\theta}}_{C}^{\top}^{I}\tilde{\mathbf{p}}_{C}^{\top}\end{pmatrix}^{\top} x~I(GIθ~⊤b~g⊤Gv~I⊤b~a⊤Gp~I⊤Iθ~C⊤Ip~C⊤)⊤的雅可比故此共21列。可以分为两部分如下公式所示 J ( J I 0 6 × 6 N ) \mathbf{J}\begin{pmatrix}\mathbf{J}_I\mathbf{0}_{6\times6N}\end{pmatrix} J(JI06×6N) 原论文给出的公式如下但是和代码对不上 J I ( C ( I G q ^ ) 0 3 × 9 0 3 × 3 I 3 0 3 × 3 − C ( I G q ^ ) ⊤ ⌊ I p ^ C × ⌋ 0 3 × 9 I 3 0 3 × 3 I 3 ) \mathbf{J}_I\begin{pmatrix}C\left(\frac{I}{G}\hat{\mathbf{q}}\right)\mathbf{0}_{3\times9}\mathbf{0}_{3\times3}\mathbf{I}_{3}\mathbf{0}_{3\times3}\\-C\left(\frac{I}{G}\hat{\mathbf{q}}\right)^\top\lfloor{}^{I}\hat{\mathbf{p}}_{C\times}\rfloor\mathbf{0}_{3\times9}\mathbf{I}_{3}\mathbf{0}_{3\times3}\mathbf{I}_{3}\end{pmatrix} JI(C(GIq^)−C(GIq^)⊤⌊Ip^C×⌋03×903×903×3I3I303×303×3I3) 代码中应该是下面这样的 J [ R c b 0 0 0 0 I 0 ( R w b t b c ) ∧ 0 0 0 I 0 R w b ] \left.J\left[\begin{array}{cccccccc}R_{cb}\mathbf{0}\mathbf{0}\mathbf{0}\mathbf{0}\mathbf{I}\mathbf{0}\\(R_{wb}t_{bc})^{\wedge}\mathbf{0}\mathbf{0}\mathbf{0}\mathbf{I}\mathbf{0}R_{wb}\end{array}\right.\right] J[Rcb(Rwbtbc)∧0000000II00Rwb]
下面求下具体的雅可比
2 扩展误差状态协方差
直接把雅可比乘进去就好 4.3.2 为什么要状态增广 IMU Propagation只改变IMU状态向量和其对应的协方差与相机无关而Measurement Updata的观测模型是残差相对于相机状态的观测模型与IMU状态没有直接关联。状态扩增就相当于相机和IMU状态之间的桥梁通过关联协方差 P I C P_{IC} PIC描述相机和IMU状态之间的关系每一个相机状态都与IMU状态形成关联这样在观测更新相机状态的同时会间接更新IMU状态。
5 状态更新
5.1 理论分析 理想数值 估计数值(预测的状态) 误差 误差状态协方差矩阵。 预测过程包括对名义状态的预测IMU 积分以及对误差状态的预测 δ x p r e d F δ x , P p r e d F P F ⊤ Q . \begin{aligned}\delta x_{\mathrm{pred}}F\delta x,\\P_{\mathrm{pred}}FPF^\topQ.\end{aligned} δxpredPpredFδx,FPF⊤Q. 考虑更新过程。假设一个抽象的传感器能够对状态变量产生观测其观测方程为抽象的 h那么可以写为 z h ( x ) v , v ∼ N ( 0 , V ) , zh(x)v,v\sim\mathcal{N}(0,V), zh(x)v,v∼N(0,V), 在 ESKF 中我们当前拥有名义状态 x 的估计以及误差状态 δx 的估计且希望更新的是误差状态因此要计算投影误差相对于误差状态的雅可比矩阵 H ∂ h ∂ δ x ∣ x p r e d ∂ δ z ∂ z ^ ∂ z ^ ∂ x ^ ∂ x ^ ∂ δ x ∂ z ^ ∂ x ^ ∂ x ^ ∂ δ x ∂ z ^ ∂ x ^ H\left.\frac{\partial h}{\partial\delta\boldsymbol{x}}\right|_{x_{\mathrm{pred}}} \\ \frac{\partial\delta\mathbf{z}}{\partial\hat{\mathbf{z}}}\mathrm{~\frac{\partial\hat{\mathbf{z}}}{\partial\hat{\mathbf{x}}}~\frac{\partial\hat{\mathbf{x}}}{\partial\delta x}} \\ \mathrm{~\frac{\partial\hat{\mathbf{z}}}{\partial\hat{\mathbf{x}}}~\frac{\partial\hat{\mathbf{x}}}{\partial\delta x}}\\ \mathrm{~\frac{\partial\hat{\mathbf{z}}}{\partial\hat{\mathbf{x}}}} H∂δx∂h xpred∂z^∂δz ∂x^∂z^ ∂δx∂x^ ∂x^∂z^ ∂δx∂x^ ∂x^∂z^ δ z z − z ^ \delta\mathbf{z}\mathbf{z}-\hat{\mathbf{z}} δzz−z^, δ z \delta\mathbf{z} δz对z求导就是单位阵观测都是正常加减法不存在旋转矩阵那种乘法同理 δ x x − x ^ \delta\mathbf{x}\mathbf{x}-\hat{\mathbf{x}} δxx−x^在处理旋转矩阵这种状态量是扰动乘法以外求导也是单位阵出于严谨可以得到对应状态p、v、R、b_g、b_a、g ∂ x ∂ δ x diag ( I 3 , I 3 , ∂ Log ( R ( Exp ( δ θ ) ) ) ∂ δ θ , I 3 , I 3 , I 3 ) . \begin{aligned}\frac{\partial x}{\partial\delta x}\operatorname{diag}(I_3,I_3,\frac{\partial\operatorname{Log}(\boldsymbol{R}(\operatorname{Exp}(\delta\boldsymbol{\theta})))}{\partial\delta\boldsymbol{\theta}},I_3,I_3,I_3).\end{aligned} ∂δx∂xdiag(I3,I3,∂δθ∂Log(R(Exp(δθ))),I3,I3,I3). 相当于连乘旋转矩阵需要用BCH近似计算实际不应该直接作为单位阵还有一点这里如果是JPL四元数那么应该是左扰动与下面有区别。 ∂ L o g ( R ( E x p ( δ θ ) ) ) ∂ δ θ J r − 1 ( R ) . \frac{\partial\mathrm{Log}(\boldsymbol{R}(\mathrm{Exp}(\delta\boldsymbol{\theta})))}{\partial\delta\boldsymbol{\theta}}\boldsymbol{J}_r^{-1}(\boldsymbol{R}). ∂δθ∂Log(R(Exp(δθ)))Jr−1(R). 代码中貌似认为是小量直接作为单位阵即 H ∂ z ^ ∂ x ^ H \mathrm{~\frac{\partial\hat{\mathbf{z}}}{\partial\hat{\mathbf{x}}}} H ∂x^∂z^ 再计算卡尔曼增益进而计算误差状态的更新过程 K P pred H ⊤ ( H P pred H ⊤ V ) − 1 , δ x K ( z − h ( x p r e d ) ) , x x p r e d δ x , P ( I − K H ) P p r e d . \begin{aligned} \boldsymbol{K} P_\text{pred}H^\top(HP_\text{pred}H^\topV)^{-1}, \\ \delta x K(z-h(\boldsymbol{x_\mathrm{pred}})), \\ \boldsymbol{x} x_{\mathrm{pred}}\delta\boldsymbol{x}, \\ \text{P} (\boldsymbol{I}-\boldsymbol{K}\boldsymbol{H})\boldsymbol{P}_{\mathrm{pred}}. \end{aligned} KδxxPPpredH⊤(HPpredH⊤V)−1,K(z−h(xpred)),xpredδx,(I−KH)Ppred. 说白了ESKF就是预测的时候对状态和误差状态都预测了但实际上只用了状态的预测值对于误差状态主要是利用了协方差和更新后的误差Δx。在更新这一块没有使用原始状态对整个方程进行更新针对不同的观测模型就有不同的雅可比矩阵H这个是最重要的 5.2 视觉观测雅可比矩阵H推导 前面已经知道H是计算投影误差相对于误差状态的雅可比矩阵 滑动窗口内状态——IMU状态、左相机状态旋转、平移 x ~ ( x ~ I ⊤ x ~ C 1 ⊤ … x ~ C N ⊤ ) ⊤ \tilde{\mathbf{x}}\begin{pmatrix}\tilde{\mathbf{x}}_I^\top\tilde{\mathbf{x}}_{C_1}^\top\dots\tilde{\mathbf{x}}_{C_N}^\top\end{pmatrix}^\top x~(x~I⊤x~C1⊤…x~CN⊤)⊤ 由于MSCKF更新使用的是不在跟踪上的点所以这个点必然在IMU状态前⾯因此关于IMU状态的雅可比就是0我们只需要计算关于相机状态的雅可比 单个路标点雅可比论文公式5 r i j z i j − z ^ i j H C i j x ~ C i H f i j G p ~ j n i j \mathbf{r}_i^j\mathbf{z}_i^j-\hat{\mathbf{z}}_i^j\mathbf{H}_{C_i}^j\tilde{\mathbf{x}}_{C_i}\mathbf{H}_{f_i}^j{}^G\tilde{\mathbf{p}}_j\mathbf{n}_i^j rijzij−z^ijHCijx~CiHfijGp~jnij 下面推导本质就是纯视觉雅可比计算可参考十四讲 当一个路标点看不见时雅可比矩阵论文公式5 r i j z i j − z ^ i j H C i j x ~ C i H f i j G p ~ j n i j \mathbf{r}_i^j\mathbf{z}_i^j-\hat{\mathbf{z}}_i^j\mathbf{H}_{C_i}^j\tilde{\mathbf{x}}_{C_i}\mathbf{H}_{f_i}^j{}^G\tilde{\mathbf{p}}_j\mathbf{n}_i^j rijzij−z^ijHCijx~CiHfijGp~jnij 如果是双目观测会翻倍但下面M指观测到路标点的次数 当最新帧有多个路标点看不见时论文公式这里没有了路标下标i r j H x j x ~ H f j G p ~ j n j \mathbf{r}^{j}\mathbf{H}_{\mathrm{x}}^{j}\tilde{\mathbf{x}}\mathbf{H}_{f}^{jG}\tilde{\mathbf{p}}_{j}\mathbf{n}^{j} rjHxjx~HfjGp~jnj 5.3 视觉观测雅可比的后续处理 5.3.1 左零空间投影 参考论文A Multi-State Constraint Kalman Filterfor Vision-aided Inertial Navigation公式24~26 对于 EKF残差线性化需要满足如下形式即残差仅与一组状态向量的误差项成线性化关系 r ≅ H δ x n r\cong H\delta xn r≅Hδxn且噪声项为与状态向量无关的零均值的高斯分布 而 MSCKF 的的残差与两个状态向量的误差项相关不满足上式的线性化形式因此不能直接应用 EKF 的测量更新流程 为了解决这个问题并让这对路标点的雅可比隐式地参与到计算进行左零空间投影具体参考后面的零空间实现实际也是利用QR分解 r 0 ( 2 M − 3 M L ) × 1 A T r 2 M × 1 ≅ A T H X 2 M × ( 15 6 N ) ⏟ H 0 X ~ ( 15 6 N ) × 1 A T n 2 M × 1 ⏟ n 0 H 0 ( 2 M − 3 ) × ( 15 6 N ) X ~ ( 15 6 N ) × 1 n 0 ( 2 M − 3 ) × 1 \begin{aligned}r_0^{(2M-3M_L)\times1}\mathrm{A}^Tr^{2M\times1}\cong\underbrace{\mathrm{A}^TH_X^{2M\times(156N)}}_{H_0}\tilde{X}^{(156N)\times1}\underbrace{\mathrm{A}^Tn^{2M\times1}}_{n_0}\\{H_0}^{(2M-3)\times(156N)}\tilde{X}^{(156N)\times1}{n_0}^{(2M-3)\times1}\end{aligned} r0(2M−3ML)×1ATr2M×1≅H0 ATHX2M×(156N)X~(156N)×1n0 ATn2M×1H0(2M−3)×(156N)X~(156N)×1n0(2M−3)×1
5.3.2 QR分解降低计算量 参考论文A Multi-State Constraint Kalman Filterfor Vision-aided Inertial Navigation公式27~30 投影完之后我们就可以得到新的残差和相应的雅可比矩阵 H 0 H_{0} H0。但是 H 0 H_{0} H0矩阵通常维度很大为了降低计算量对 H 0 H_{0} H0进行QR分解 QR分解会得到一个正交矩阵和一个上三角矩阵正交矩阵的转置和其本身的乘积是单位矩阵且该矩阵的所有列向量彼此正交内积为0 H 0 ( 2 M − 3 ) × ( 15 6 N ) [ Q 1 Q 2 ] [ T H 0 ] {H_0}^{(2M-3)\times(156N)}[Q_1\quad Q_2]\begin{bmatrix}T_H\\0\end{bmatrix} H0(2M−3)×(156N)[Q1Q2][TH0] 对上面新的残差左边乘以 [ Q 1 T Q 2 T ] \left.\left[\begin{array}{cc}\mathbf{Q}_1^T\\\mathbf{Q}_2^T\end{array}\right.\right] [Q1TQ2T],得到 [ Q 1 T r 0 Q 2 T r 0 ] [ T H 0 ] X ~ [ Q 1 T n 0 Q 2 T n 0 ] \begin{bmatrix}Q_1^Tr_0\\Q_2^Tr_0\end{bmatrix}\begin{bmatrix}T_H\\0\end{bmatrix}\tilde{X}\begin{bmatrix}Q_1^Tn_0\\Q_2^Tn_0\end{bmatrix} [Q1Tr0Q2Tr0][TH0]X~[Q1Tn0Q2Tn0] 这个过程和左零空间投影很像这里相当于取第一行对应的矩阵块左零空间则是取第二行对应的矩阵块。总之都是利用了QR分解
5.4 更新 参考论文A Multi-State Constraint Kalman Filterfor Vision-aided Inertial Navigation公式31~33 6 左零空间投影
6.1 为什么要进行左零空间投影 要符合EKF的残差线性化形式 视觉slam中得到重投影误差r之后做更新前需要计算重投影误差相对于位姿与三维点的雅可比利用雅可比来更新位姿以及三维点。
有⼀种模式只优化位姿那是认为三维点精度很高没有误差即使这样点还是会在后端优化。所以重投影误差中是包含着三维点的误差信息的msckf的思想就是做不带三维点坐标的状态更新但是并没有完全不考虑三维点的影响这个思想是与边缘化⼀样通过“移动”的手段。 在上面描述中出现了两个雅可比矩阵⼀个是重投影误差相对于状态量的雅可比位姿速度bias等⼀切参与重投影误差的量它的行数等于误差维度也就是2n个n为点数例如我们的双目msckf就是4n因为⼀个点在左右目都有观测列数等于状态量维度。重投影误差相对于三维点的雅可比同理。
现在为了将这部分不参与计算并让这部分隐式地参与到计算如何做要注意这里并不能直接向上面⼀样把三维点固定死因为三维点的误差是不能忽略的。 残差 r i j z i j − z ^ i j H C i j x ~ C i H f i j p ~ j n i j \mathbf{r}_{i}^{j}\mathbf{z}_{i}^{j}-\hat{\mathbf{z}}_{i}^{j}\mathbf{H}_{C_{i}}^{j}\tilde{\mathbf{x}}_{C_{i}}\mathbf{H}_{f_{i}}^{j}\tilde{\mathbf{p}}_{j}\mathbf{n}_{i}^{j} rijzij−z^ijHCijx~CiHfijp~jnij 我们找到关于对特征雅可比 H f i j {H}_{f_{i}}^{j} Hfij的左零空间 V V Vensure the uncertainty of p ~ j \tilde{\mathbf{p}}_{j} p~jdoes not affect the residual确保哪些只计算一次保持不变的三维点误差很大影响到残差的计算 r o j V ⊤ r j V ⊤ H x j x ~ V ⊤ n j H x , o j x ~ n o j \mathbf{r}_o^j\mathbf{V}^\top\mathbf{r}^j\mathbf{V}^\top\mathbf{H}_\mathbf{x}^j\tilde{\mathbf{x}}\mathbf{V}^\top\mathbf{n}^j\mathbf{H}_{\mathbf{x},o}^j\tilde{\mathbf{x}}\mathbf{n}_o^j rojV⊤rjV⊤Hxjx~V⊤njHx,ojx~noj 找到⼀个矩阵V乘在等式左右将三维点的那项变成0矩阵那么只需要更新状态即可同时点也通过V矩阵融入到状态中数学上讲通过A矩阵将前两项投影到了第三项的零空间上概率上讲这种操作使得与三维点无关。 左零空间与零空间区别 零空间Null Space 零空间也称为核kernel是一个线性变换或矩阵中所有映射到零向量的向量组成的空间。对于矩阵 A其零空间是 A x 0 Ax0 Ax0所有解的向量组成的空间 左零空间Left Null Space 左零空间是一个矩阵的转置的零空间。对于矩阵 A其左零空间是 y T A 0 y^TA0 yTA0所有解的向量组成的空间
零空间维度 矩阵A的秩 矩阵维度n
6.2 如何计算零空间矩阵V 对于上面两个雅可比维度分别如下4是因为双目单目观测为2所以双目是4.n表示了当前观测z被观测的帧数 H C i j ( J 1 H 1 J 2 R ⊤ H 1 ) 4 n ∗ 6 , H f i j ( J 1 H 2 J 2 R ⊤ H 2 ) 4 n ∗ 3 \left.\mathbf{H}_{C_i}^j\left(\begin{array}{c}\mathbf{J}_1\mathbf{H}_1\\\mathbf{J}_2\mathbf{R}^\top\mathbf{H}_1\end{array}\right.\right)_{4n*6},\quad\mathbf{H}_{f_i}^j\left(\begin{array}{c}\mathbf{J}_1\mathbf{H}_2\\\mathbf{J}_2\mathbf{R}^\top\mathbf{H}_2\end{array}\right)_{4n*3} HCij(J1H1J2R⊤H1)4n∗6,Hfij(J1H2J2R⊤H2)4n∗3 把这个雅可比通过QR分解分出两个矩阵正交矩阵 Q 和一个上三角矩阵 R 的乘积然后对两个矩阵分块处理可以得到类似下图矩阵块 因为Q是正交矩阵所以Q2中任意一列乘以Q1中一列为0所以Q2的转置实际上就是我们要找的左零空间矩阵注意到一个正交矩阵Q乘以正交矩阵的转置就是单位阵 δ r J x δ X J p δ P δ r J x δ X [ Q 1 Q 2 ] [ R 1 0 ] δ P [ Q 1 T Q 2 T ] δ r [ Q 1 T Q 2 T ] J x δ X [ R 1 0 ] δ P \begin{gathered} \delta\mathbf{r}\mathbf{J_x}\delta\mathbf{X}\mathbf{J_p}\delta\mathbf{P} \\ \left.\delta\mathbf{r}\mathbf{J}_\mathbf{x}\delta\mathbf{X}\left[\begin{array}{ll}\mathbf{Q}_1\mathbf{Q}_2\end{array}\right.\right]\left[\begin{array}{ll}\mathbf{R}_1\\\mathbf{0}\end{array}\right]\delta\mathbf{P} \\ \left.\left[\begin{array}{l}\mathbf{Q_1}^T\\\mathbf{Q_2}^T\end{array}\right.\right]\delta\mathbf{r}\left[\begin{array}{ll}\mathbf{Q_1}^T\\\mathbf{Q_2}^T\end{array}\right]\mathbf{J_x}\delta\mathbf{X}\left[\begin{array}{ll}\mathbf{R_1}\\\mathbf{0}\end{array}\right]\delta\mathbf{P} \end{gathered} δrJxδXJpδPδrJxδX[Q1Q2][R10]δP[Q1TQ2T]δr[Q1TQ2T]JxδX[R10]δP 在说下为什么Q2就是左零空间矩阵呢看下面矩阵实际上可以分为两行如果单独拿出Q2对应的哪一行关于路标点的那一部分对应是0
