卡尔曼滤波

理论知识

状态空间表达式

已知系统的状态方程为：
$$x_k=A{x}_{k-1}+B{u}_k+w_k$$
系统的观测方程为：
$$y_k=C{x}_k+v_k$$
故系统的结构图为：

其中，系统的模型误差为：
$$w_k\sim N(0,Q_k)$$
系统的观测误差为：
$$v_k\sim N(0,R_k)$$

卡尔曼滤波

卡尔曼滤波是使用上一次的最优结果来预测当前的值，同时使用观测值修正当前的值，得到最优的结果。

卡尔曼滤波的预测公式为：
$${\hat{x}}_t^{-}=F{\hat{x}}_{t-1}+B{u}_{t-1}$$

$$P_t^{-}=F{P}_{t-1}{F}^T+Q$$

卡尔曼滤波的更新公式为：
$$k_t=\frac{P_t^{-}{H}^T}{H{P}_t^{-}{H}^T+R}$$

$${\hat{x}}_t={\hat{x}}_t^{-}+k_t(z_t-H{\hat{x}}_t^{-})$$

$$P_t=(I-k_{t}H)P_t^{-}$$

实践测试

背景假设

假定有一个小车正在进行匀速直线运动，其加速度为1米每二次方秒，测量频率为10Hz，即每0.1秒测量一次，小车的初始速度为0，使用卡尔曼滤波对小车的位置进行处理。

小车位置

小车理论位置

根据物理中的运动学公式，可以求得小车的位置，求得的小车位置和时间关系为：

小车实际位置

在小车实际运动以及小车实际位置获取时，小车的运动存在误差，包括小车运动时产生的误差和小车观测时产生的误差，故小车实际位置和时间关系为：

卡尔曼滤波

滤波过程

定义系统的状态量为：
$$x=\left[\begin{array}{c}pos\\ v\end{array}\right]$$
其中，pos为小车的位置，v为小车的速度。

定义系统的采样时间为dt，单位为秒；定义系统的加速度为a，单位为米每二次方秒；定义系统的观测误差为R；定义系统的运行误差的方差为：
$$Q=\left[\begin{array}{cc}{Q}_{pos}& 0\\ 0& Q_v\end{array}\right]$$
系统的先验值为：
$${\hat{x}}_t^{-}=\left[\begin{array}{cc}1& dt\\ 0& 1\end{array}\right]\cdot x_{t-1}+\left[\begin{array}{c}\frac{1}{2}\times d{t}^2\\ dt\end{array}\right]\cdot a$$
更新误差协方差：
$$P_t^{-}=\left[\begin{array}{cc}1& dt\\ 0& 1\end{array}\right]\cdot P_t\cdot \left[\begin{array}{cc}1& 0\\ dt& 1\end{array}\right]+\left[\begin{array}{cc}{Q}_{pos}& 0\\ 0& Q_v\end{array}\right]$$

在获取测量值时，实际使用直接读取传感器的输出值，在本测试中读取来自测量值矩阵的数据。在本次测试中，测量值公式写为：
$$z(t)=pos=\left[\begin{array}{cc}1& 0\end{array}\right]\cdot \left[\begin{array}{c}pos\\ v\end{array}\right]+V$$
卡尔曼滤波系数为：
$$k_t=\frac{P_t^{-}\cdot \left[\begin{array}{c}1\\ 0\end{array}\right]}{\left[\begin{array}{cc}1& 0\end{array}\right]\cdot P_t^{-}\cdot \left[\begin{array}{c}1\\ 0\end{array}\right]+R}$$
由此可以获得卡尔曼预测值为：
$${\hat{x}}_t={\hat{x}}_t^{-}+k_t[z(t)-\left[\begin{array}{cc}1& 0\end{array}\right]\cdot {\hat{x}}_t^{-}]$$
最后更新协方差为：
$$P_t=[\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right]-k_t\cdot \left[\begin{array}{cc}1& 0\end{array}\right]]\cdot P_t^{-}$$

滤波结果

卡尔曼滤波的结果为：

执行结果

卡尔曼滤波执行的最终结果为