卡尔曼滤波学习(7)_卡尔曼滤波小车位置运动-优快云博客

本文链接：https://blog.youkuaiyun.com/lglrkx/article/details/136741679

前面已经完成了卡尔曼滤波所有公式和参数的推导，现在我们举例来实践卡尔曼滤波。

设一辆车正在进行匀速运动，当前位置 $x_{1,k}$ ，速度 $x_{2,k}$ ，单位时间为 $\Delta T$ ， $w_{1,k-1}$ 、 $w_{2,k-1}$ 为过程噪声，那么 $k$ 时刻位置、速度和 $k-1$ 时刻的位置、速度的关系为：

$x_{1,k}=x_{1,k-1}+x_{2,k-1}*\Delta T+w_{1,k-1}$ (1)

$x_{2,k} = x_{2,k-1}+w_{2,k-1}$ (2)

现在有GPS对该车位置和速度进行观测，设位置观测值 $Z_{1,k}$ ，速度观测值 $Z_{2,k}$ ， $v_{1,k}$ 、 $v_{2,k}$ 为观测噪声，它们与 $k$ 时刻位置和速度的关系为：

$Z_{1,k} = x_{1,k}+v_{1,k}$ (3)

$Z_{2,k} = x_{2,k}+v_{2,k}$ (4)

将(1)和(2)，(3)和(4)分别写成矩阵形式：

$\begin{bmatrix}x_{1,k} & \\ x_{2,k} & \end{bmatrix}=\begin{bmatrix}1,\Delta T & \\ 0,1 & \end{bmatrix}\begin{bmatrix}x_{1,k-1} & \\ x_{2,k-1} & \end{bmatrix}+\begin{bmatrix}w_{1,k-1} & \\ w_{2,k-1} & \end{bmatrix}$

即： $X_k = AX_{k-1}+Q$ (5)

$\begin{bmatrix}z_{1,k} & \\ z_{2,k} & \end{bmatrix} = \begin{bmatrix}1,0 & \\ 0,1 & \end{bmatrix}\begin{bmatrix}x_{1,k} & \\ x_{2,k} & \end{bmatrix}+\begin{bmatrix}v_{1,k} & \\ v_{2,k} & \end{bmatrix}$

即： $Z_k = HX_k+R$ (6)

我们已经得到该系统的状态矩阵方程和测量矩阵方程，下面我们代入卡尔曼滤波公式，可得到：

预测：

$\hat{X}_k^- = A\hat{X}_{k-1}$ (7)

$P_k^- = AP_{k-1}A^T+Q$ (8)

校正:

$K_k = \frac{P_k^-H^T}{HP_k^-H^T+R}$ (9)

$\hat{X}_k=\hat{X}_{k}^-+K_k(Z_k-\hat{X}_{k}^-)$ (10)

$P_k = (I-K_kH)P_k^-$ (11)

在初始0时刻我们假设该时刻 $\hat{X}_0=\begin{bmatrix}0 \\ 0.1 \end{bmatrix}$ ， $Q = \begin{bmatrix}0.1,0 & \\ 0,0.1 & \end{bmatrix}$ ， $R=\begin{bmatrix}0.1,0 & \\ 0,0.1 & \end{bmatrix}$ ， $P=\begin{bmatrix}1,0 & \\ 0,1 & \end{bmatrix}$ 。矩阵 $A$ 、 $H$ 已经已知，下面只要有测量值的输入，便可以进行卡尔曼滤波，寻找k时刻的最估计值，过程噪声Q和观测噪声R的选取会对影响到后验估计值更倾向于先验估计值还是测量值。

下面是该例子的C代码【免费】卡尔曼滤波-C语言卡尔曼滤波学习(7)http://t.csdnimg.cn/4ubGO资源-优快云文库

学习笔记参考资料【【卡尔曼滤波器】5_直观理解与二维实例【包含完整的EXCEL代码】】 https://www.bilibili.com/video/BV1dV411B7ME/?share_source=copy_web&vd_source=c29456ffa88bc7559f8ffbe6f8e8f7a5