卡尔曼滤波总结
卡尔曼滤波
建模
\[x_{k}=Ax_{k-1}+Bu_{k-1}+w_{k-1} \quad P(w) \sim N(0,Q)\\
z_{k}=Hx_{k}+v_{k} \quad p(v) \sim N(0,R)
\]
预测
\[先验:\hat{x}_{k}^{-}=A \hat{x}_{k-1}+B u_{k-1} \\
先验协方差: P_{k}^{-}=A P_{k-1} A^{\top}+Q
\]
更新
\[后验:\hat{x}_{k}=\hat{x}_{k}^{-}+K_{k}\left(z_{k}-H \hat{x}_{k}^{-}\right) \\
后验协方差: P_{k}=\left(I-K_{k} H\right) P_{k}^{-}
\]
其中
\[卡尔曼增益:{K_k} = \frac{{P_k^ - {H^ \top }}}{{HP_k^ - {H^ \top } + R}}
\]
拓展卡尔曼滤波
针对非线性的模型
建模
\[x_{k}=f(x_{k-1},u_{k-1},w_{k-1}) \quad P(w) \sim N(0,Q)\\
z_{k}=h(x_{k},v_{k}) \quad p(v) \sim N(0,R)
\]
线性化
\(x_{k}\) 在 \(\hat{x_{k-1}}\) 处线性化（泰勒展开）
\[x_{k}=f(x_{k-1},u_{k-1},0)+A(x_{k-1}-\hat{x_{k-1}})+W w_{k-1} \\
A = \frac{\partial f}{\partial x}|_{\hat{x_{k-1}}, u_{k-1}}\\
W = \frac{\partial f}{\partial w}|_{(\hat{x_{k-1}}, u_{k-1})}
\]
\(z_{k}\) 在 \(\hat x_k^ - = f\left( {{{\hat x}_{k - 1}},{u_{k - 1}},0} \right)\) 处线性化（泰勒展开）
\[z_{k}=h(\hat {x_k}^-,0)+H(x_{k}-\hat {x_k}^-)+V v_{k} \\
H = \frac{\partial h}{\partial x}|_{\hat {x_k}^-}\\
V = \frac{\partial h}{\partial w}|_{\hat {x_k}^-}
\]
预测
\[先验: \hat x_k^ - = f\left( {{{\hat x}_{k - 1}},{u_{k - 1}},0} \right)\\
先验协方差: P_k^ - = AP_{k - 1}^{}{A^ \top } + WQ{W^ \top }
\]
更新
\[后验: {{\hat x}_k} = \hat x_k^- + {K_k}\left( {{z_k} - h\left( {\hat x_k^ - ,0} \right)} \right)\\
后验协方差: P_k^{} = \left( {I - {K_k}H} \right)P_k^-
\]
其中
\[卡尔曼增益：{K_k} = \frac{{P_k^ - {H^ \top }}}{{HP_k^ - {H^ \top } + VR{V^ \top }}}
\]
状态误差卡尔曼滤波
误差状态卡尔曼滤波是对误差状态的预测和更新，再将误差更新到名义状态变量
预测
\[先验名义状态: \hat x_k^ - = f\left( {{{\hat x}_{k - 1}},{u_{k - 1}},0} \right)\\
误差状态：\delta \hat x_k^ - = F_x \delta \hat x_{k-1} \\
误差状态先验协方差: P_k^ - = F_x P_{k - 1}{F_x^ \top } + F_iQ{F_i^ \top }
\]
更新
\[误差状态更新：\delta x = K (z - h\left( {\hat x_k^ - ,0} \right)) \\
后验： {{\hat x}_k} = \hat x_k^- + \delta x \\
后验协方差: P_k^{} = \left( {I - {K_k}H} \right)P_k^-
\]
其中
\[卡尔曼增益：{K_k} = \frac{{P_k^ - {H^ \top }}}{{HP_k^ - {H^ \top } + VR{V^ \top }}}
\]
