预测协方差矩阵

本文为原创文章，转载请注明出处---https://blog.youkuaiyun.com/q_z_r_s

机器感知 一个专注于SLAM、三维重建、机器视觉等相关技术文章分享的公众号

假设机器人位姿为3维，每个landmark为2维，EKF中的状态向量为：
$$x={\left[\begin{array}{ccccc}{x}_v& y_1& y_2& \cdots & y_n\end{array}\right]}^T$$
运动方程和观测方程为：
$$\begin{gathered} x_t=f(x_{t-1},u_t,v_t) \\ {z}_t=h(x_t,{\omega }_t) \end{gathered}$$
故有：
$$G_t=I+\left[\begin{array}{cc}{\frac{\partial f}{\partial x}}_{3\times 3}& 0_{3\times 2n}\\ 0_{2n\times 3}& 0_{2n\times 2n}\end{array}\right]=\left[\begin{array}{cc}{F}_{3\times 3}& 0_{3\times 2n}\\ 0_{2n\times 3}& I_{2n\times 2n}\end{array}\right]$$
因此，协方差矩阵的预测值为：
$$\begin{array}{rl}{\overline{\Sigma }}_t& =G_t{\Sigma }_{t-1}{G}_t^T+R_t\\ & =\left[\begin{array}{cc}{F}_{3\times 3}& 0_{3\times 2n}\\ 0_{2n\times 3}& I_{2n\times 2n}\end{array}\right]{\Sigma }_{t-1}\left[\begin{array}{cc}{F}_{3\times 3}^T& 0_{3\times 2n}\\ 0_{2n\times 3}& I_{2n\times 2n}\end{array}\right]+R_t\\ & =\left[\begin{array}{cc}{F}_{3\times 3}& 0_{3\times 2n}\\ 0_{2n\times 3}& I_{2n\times 2n}\end{array}\right]\left[\begin{array}{cc}{\Delta }_{3\times 3}^1& {\Delta }_{3\times 2n}^2\\ {\Delta }_{2n\times 3}^3& {\Delta }_{2n\times 2n}^4\end{array}\right]\left[\begin{array}{cc}{F}_{3\times 3}^T& 0_{3\times 2n}\\ 0_{2n\times 3}& I_{2n\times 2n}\end{array}\right]+R_t\\ & =\left[\begin{array}{cc}{F}_{3\times 3}& 0_{3\times 2n}\\ 0_{2n\times 3}& I_{2n\times 2n}\end{array}\right]\left[\begin{array}{cc}{\Delta }_{3\times 3}^1{F}_{3\times 3}^T& {\Delta }_{3\times 2n}^2\\ {\Delta }_{2n\times 3}^3{F}_{3\times 3}^T& {\Delta }_{2n\times 2n}^4\end{array}\right]+R_t\\ & =\left[\begin{array}{cc}{F}_{3\times 3}{\Delta }_{3\times 3}^1{F}_{3\times 3}^T& F_{3\times 3}{\Delta }_{3\times 2n}^2\\ {\Delta }_{2n\times 3}^3{F}_{3\times 3}^T& {\Delta }_{2n\times 2n}^4\end{array}\right]+R_t\end{array}$$

综上，可以得出，预测协方差矩阵时，只要与机器人位姿相关的的协方差矩阵块都是有变化的，而仅与landmark相关的协方差矩阵块完全没有任何变化。