关于 kalman filter 中的参数整定

ref:

• http://blog.sina.com.cn/s/blog_40edfdc90102ww2o.html
• http://blog.sina.com.cn/s/blog_40edfdc90102xfex.html
• http://blog.sina.com.cn/s/blog_40edfdc90102vhmw.html
• http://blog.sina.com.cn/s/blog_40edfdc90102v5oa.html
• https://en.wikipedia.org/wiki/Kalman_filter

状态与协方差的更新方式

• 从 Kalman 滤波的公式中可以看出，状态与协方差的更新是相互独立的；所以在实现的过程中需要注意保证其协方差与状态的匹配；

Predict

• Predicted (a priori) state estimate

$${\hat{\mathbf{x}}}_{k\mid k-1}={\mathbf{F}}_k{\hat{\mathbf{x}}}_{k-1\mid k-1}+{\mathbf{B}}_k{\mathbf{u}}_k{\hat{\mathbf{x}}}_{k\mid k-1}={\mathbf{F}}_k{\hat{\mathbf{x}}}_{k-1\mid k-1}+{\mathbf{B}}_k{\mathbf{u}}_k$$

• Predicted (a priori) error covariance

$${\mathbf{P}}_{k\mid k-1}={\mathbf{F}}_k{\mathbf{P}}_{k-1\mid k-1}{\mathbf{F}}_k^{\text{T}}+{\mathbf{Q}}_k{\mathbf{P}}_{k\mid k-1}={\mathbf{F}}_k{\mathbf{P}}_{k-1\mid k-1}{\mathbf{F}}_k^{\text{T}}+{\mathbf{Q}}_k$$

Update

• Innovation or measurement pre-fit residual

$${\tilde{\mathbf{y}}}_k={\mathbf{z}}_k-{\mathbf{H}}_k{\hat{\mathbf{x}}}_{k\mid k-1}{\tilde{\mathbf{y}}}_k={\mathbf{z}}_k-{\mathbf{H}}_k{\hat{\mathbf{x}}}_{k\mid k-1}$$

• Innovation (or pre-fit residual) covariance

$${\mathbf{S}}_k={\mathbf{R}}_k+{\mathbf{H}}_k{\mathbf{P}}_{k\mid k-1}{\mathbf{H}}_k^{\text{T}}{\mathbf{S}}_k={\mathbf{R}}_k+{\mathbf{H}}_k{\mathbf{P}}_{k\mid k-1}{\mathbf{H}}_k^{\text{T}}$$

• Optimal Kalman gain

$${\mathbf{K}}_k={\mathbf{P}}_{k\mid k-1}{\mathbf{H}}_k^{\text{T}}{\mathbf{S}}_k^{-1}{\mathbf{K}}_k={\mathbf{P}}_{k\mid k-1}{\mathbf{H}}_k^{\text{T}}{\mathbf{S}}_k^{-1}$$

• Updated (a posteriori) state estimate
  $${\hat{\mathbf{x}}}_{k\mid k}={\hat{\mathbf{x}}}_{k\mid k-1}+{\mathbf{K}}_k{\tilde{\mathbf{y}}}_k{\hat{\mathbf{x}}}_{k\mid k}={\hat{\mathbf{x}}}_{k\mid k-1}+{\mathbf{K}}_k{\tilde{\mathbf{y}}}_k$$

• Updated (a posteriori) estimate covariance

$${\mathbf{P}}_{k|k}=\left(\mathbf{I}-{\mathbf{K}}_k{\mathbf{H}}_k\right){\mathbf{P}}_{k|k-1}{\mathbf{P}}_{k|k}=\left(\mathbf{I}-{\mathbf{K}}_k{\mathbf{H}}_k\right){\mathbf{P}}_{k|k-1}$$

• Updated (a posteriori) estimate covariance Joseph form 数值稳定

$${\mathbf{P}}_{k|k}=\left(\mathbf{I}-{\mathbf{K}}_k{\mathbf{H}}_k\right){\mathbf{P}}_{k|k-1}{\left(\mathbf{I}-{\mathbf{K}}_k{\mathbf{H}}_k\right)}^{\text{T}}+{\mathbf{K}}_k{\mathbf{R}}_k{\mathbf{K}}_k^{\text{T}}{\mathbf{P}}_{k|k}=\left(\mathbf{I}-{\mathbf{K}}_k{\mathbf{H}}_k\right){\mathbf{P}}_{k|k-1}{\left(\mathbf{I}-{\mathbf{K}}_k{\mathbf{H}}_k\right)}^{\text{T}}+{\mathbf{K}}_k{\mathbf{R}}_k{\mathbf{K}}_k^{\text{T}}$$

• Measurement post-fit residual

$${\tilde{\mathbf{y}}}_{k\mid k}={\mathbf{z}}_k-{\mathbf{H}}_k{\hat{\mathbf{x}}}_{k\mid k}{\tilde{\mathbf{y}}}_{k\mid k}={\mathbf{z}}_k-{\mathbf{H}}_k{\hat{\mathbf{x}}}_{k\mid k}$$

残差校验

• 残差校验部分主要的作用是排除掉有害数据的影响；
• 对于野值、或者某些模态下的有害数据，应该通过 残差校验 或其他方式，防止其进入滤波器；
• 主要方法包括 ${\chi }^2$ 校验、观测残差、残差导数等。

噪声模型的限制

• 对于有残差校验的滤波器，应该限制噪声上限，防止大噪声量测污染状态量；
• 对于某些有害的量测数据，应防止其进入滤波器；（某些状态下的量测对状态有害无益）
• 考虑使用信噪比描述量测，从而切除有害量测；

---

KeyPoint

• 不要迷信状态协方差对状态的不确定性描述，如果没有相应的自适应措施，协方差对状态的描述越精确，精度越高，鲁棒性越差；
• 参数的整定主要保证量测更新时，状态和协方差能够合理的分配新息，同时，需注意保证适量的过程噪声以保证系统鲁棒性。