自动驾驶：卡尔曼滤波

卡尔曼滤波

概述

卡尔曼滤波（Kalman filtering，KF）是一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。由于建模的不确定性导致系统本身存在过程噪声，以及利用传感器进行观测存在测量噪声，假设两者均服从正态分布，卡尔曼滤波在线性状态空间表示的基础上对有噪声的输入和观测信号进行处理，求取系统状态最优值。所以卡尔曼滤波适用于线性、离散和有限维系统。

卡尔曼滤波是目前应用最为广泛的滤波方法，在通信，导航，制导与控制等多领域得到了较好的应用。

公式及含义

假设线性系统的状态方程为：

$$x_k=A{x}_{k-1}+B{u}_{k-1}+w_{k-1}$$
其中，

• $x_k$和$x_{k-1}$分别是k时刻和k-1时刻的状态值；
• $A$是状态转移矩阵；
• $B$是控制输入矩阵；
• $u_{k-1}$是k-1时刻的输入；
• $w_{k-1}$是k-1时刻的过程噪声，服从期望为 $0$，协方差矩阵为 $Q$ 的正态分布（高斯分布）。

观测方程为:
$$z_k=H{x}_k+v_k$$
其中，

• $x_k$分别是k时刻的状态值；
• $z_k$是k时刻的测量值（观测值）
• $v_k$是k时刻的测量噪声（观测噪声），服从期望为 $0$，协方差矩阵为 $R$ 的正态分布（高斯分布）；
• H是状态观测矩阵。

卡尔曼滤波器时间更新方程：
$$\begin{gathered} {\hat{x}}_k^{-}=A{\hat{x}}_{k-1}+B{u}_{k-1} \\ {P}_k^{-}=A{P}_{k-1}{A}^T+Q \end{gathered}$$

卡尔曼滤波器状态更新方程：
$$\begin{gathered} K_k=\frac{P_k^{-}{H}^T}{H{P}_k^{-}{H}^T+R} \\ {\hat{x}}_k={\hat{x}}_k^{-}+K_k(z_k-H{\hat{x}}_k^{-}) \\ {P}_k=(I-K_{k}H)P_k^{-} \end{gathered}$$

其中，

• ${\hat{x}}_k$和${\hat{x}}_{k-1}$分别是k时刻和k-1时刻的后验估计值，即更卡尔曼滤波后的最优估计值；
• ${\hat{x}}_k^{-}$是k时刻的先验估计值，是卡尔曼滤波的中间结果之一；
• $P_k$和$P_{k-1}$是k时刻和k-1时刻的后验估计协方差（即${\hat{x}}_k$和${\hat{x}}_{k-1}$的协方差），表示后验估计的不确定度；
• $P_k^{-}$是k时刻的先验估计协方差，是卡尔曼滤波的中间结果之一；
• $K_k$是卡尔曼增益系数，范围为；
• $(z_k-H{\hat{x}}_k^{-})$表示实际观测和预测观测的残差，和卡尔曼增益一起修正先验估计，得到后验估计值。

扩展卡尔曼滤波

概述

扩展卡尔曼滤波（Eetend Kalman Filtering, EKF）是对非线性系统状态方程进行线性化，再应用经典卡尔曼滤波算法对系统状态进行最优估计。

公式及含义

假非线性系统的状态方程为：
$$x_k=f(x_{k-1},u_{k-1},w_{k-1})$$
其中，

• $x_k$和$x_{k-1}$分别是k时刻和k-1时刻的状态值；
• $u_{k-1}$是k-1时刻的输入；
• $w_{k-1}$是k-1时刻的过程噪声，服从期望为 $0$，协方差矩阵为 $Q$ 的正态分布（高斯分布）。

观测方程为:
$$z_k=h(x_k,v_k)$$
其中，

• $x_k$分别是k时刻的状态值；
• $z_k$是k时刻的测量值（观测值）
• $v_k$是k时刻的测量噪声（观测噪声），服从期望为 $0$，协方差矩阵为 $R$ 的正态分布（高斯分布）；

由于卡尔曼滤波只能应用于线性系统，需要对非线性系统进行线性化。

在$x={\hat{x}}_{k-1}$处进行线性化后的状态方程为：
$$x_k={\tilde{x}}_k+A(x_{k-1}-{\hat{x}}_{k-1})+W_k{w}_{k-1}$$
其中，

• ${\tilde{x}}_k=f({\hat{x}}_{k-1},u_{k-1},w_{k-1})$，由于$w_{k-1}$未知，一般简化设置为0，所以${\tilde{x}}_k=f({\hat{x}}_{k-1},u_{k-1},0)$
• $A=\frac{\partial f}{\partial x}{|}_{{\hat{x}}_{k-1},u_{k-1}}$
• $W_k=\frac{\partial f}{\partial w}{|}_{{\hat{x}}_{k-1},u_{k-1}}$

在$x={\tilde{x}}_k$处进行线性化后的观测方程为：
$$z_k={\tilde{z}}_k+H_k(x_k-{\tilde{x}}_k)+V_k{v}_k$$
其中，

• ${\tilde{z}}_k=h({\tilde{x}}_k,v_k)$，由于$v_k$未知，一般简化设置为0，所以${\tilde{z}}_k=h({\tilde{x}}_k,0)$
• $H_k=\frac{\partial h}{\partial x}{|}_{{\tilde{x}}_k}$
• $V_k=\frac{\partial h}{\partial v}{|}_{{\tilde{x}}_k}$

扩展卡尔曼滤波器时间更新方程：
$$\begin{gathered} {\hat{x}}_k^{-}=f({\hat{x}}_{k-1},u_{k-1},0) \\ {P}_k^{-}=A{P}_{k-1}{A}^T+W_k{Q}_{k-1}{W}_k^T \end{gathered}$$

扩展卡尔曼滤波器状态更新方程：
$$\begin{gathered} K_k=\frac{P_k^{-}{H}_k^T}{H_k{P}_k^{-}{H}_k^T+V_k{R}_k{V}_k^T} \\ {\hat{x}}_k={\hat{x}}_k^{-}+K_k(z_k-h({\hat{x}}_k^{-},0)) \\ {P}_k=(I-K_k{H}_k)P_k^{-} \end{gathered}$$

Python代码实现

参考pykalman官方网站：https://pykalman.github.io/