控制原理 | 详细总结卡尔曼滤波原理+具体案例分析

0 专栏介绍

🔥附C++/Python/Matlab全套代码🔥课程设计、毕业设计、创新竞赛必备！详细介绍全局规划(图搜索、采样法、智能算法等)；局部规划(DWA、APF等)；曲线优化(贝塞尔曲线、B样条曲线等)。

🚀详情：图解自动驾驶中的运动规划(Motion Planning)，附几十种规划算法

---

1 状态观测器

在工程上，部分状态参数测量的成本过高，或是用现有仪器无法测得，此时需要引入状态观测器。状态观测器以原系统输入、输出为输入，输出估计的状态变量。

原系统状态描述：
$$\{\begin{array}{l}{x}_k=A{x}_{k-1}+B{u}_{k-1}\\ y_k=C{x}_k\end{array}$$

状态观测器状态描述：
$$\{\begin{array}{l}{\tilde{x}}_k=A{\tilde{x}}_{k-1}+B{u}_{k-1}+f\left[y_{k-1}-{\tilde{y}}_{k-1}\right]\\ {\tilde{y}}_k=C{\tilde{x}}_k\end{array}$$

定义误差向量$e_k=x_k-{\tilde{x}}_k$，则两系统状态方程相减得

$$系统误差传递方程e_k=\left(A-fC\right)e_{k-1}$$

其通解为矩阵指数函数，所以当$A-fC$特征值小于0时，误差向量$e_k$各分量均趋于0，即状态观测器能直接估计原系统状态。

本质上，状态观测器是针对状态空间方程描述的确定系统，从错误的系统状态估计值不断收敛到正确的系统状态估计值的数学模型。

2 状态滤波器

实际的系统往往是随机过程，状态描述为：

$$\{\begin{array}{l}{x}_k=A{x}_{k-1}+B{u}_{k-1}+w_{k-1}\\ y_k=C{x}_k+v_k\end{array}$$

其中$w_k$称为过程噪声，主要由系统运行过程中的外力导致，如旋翼飞行器运动过程中受到的阵风；$v_k$称为测量噪声，主要由系统测量过程中测量仪器误差、精度不足导致。

本质上，状态滤波器是针对随机状态空间方程描述的随机系统，从不确定观测中提取信息，得到系统状态最优估计的数学模型。当$w_k$与$v_k$为高斯白噪声且相互独立时，状态滤波器为卡尔曼滤波器。

状态滤波器的核心是通过贝叶斯原理不断调整滤波器增益矩阵，以减小随机干扰，使估计的系统状态趋近于真实状态；而状态观测器的核心是通过极点配置确定观测器增益矩阵，使估计的状态跟踪当前系统的状态。

一般地，状态滤波器性能优于状态观测器，但在实际系统中，若随机噪声非高斯噪声，或有若干非线性环节，按常规噪声模型建立的状态滤波器可能发散，此时则更需要状态观测器的稳定性。

3 卡尔曼滤波器

考虑一个随机系统：

$$\{\begin{array}{l}{x}_k=A{x}_{k-1}+B{u}_{k-1}+w_{k-1}\\ z_k=C{x}_k+v_k\end{array}$$

其中$w_{k}N\left(0,Q\right)$、$v_{k}N\left(0,R\right)$，且$w_k$与$v_k$相互独立。

忽略噪声可建立该系统的先验数学模型(噪声仅满足随机分布，无法建模)：

$$\text{状态预测方程}\{\begin{array}{l}{\hat{x}}_k^{-}=A{\hat{x}}_{k-1}+B{u}_{k-1}\\ {\hat{z}}_k^{-}=C{\hat{x}}_k\end{array}$$

基于上述系统，从贝叶斯方法引出卡尔曼滤波器状态更新方程：

$$\text{状态更新方程}:{\hat{x}}_k={\hat{x}}_k^{-}+K_k\left(z_k-{\hat{z}}_k^{-}\right)$$

其中${\hat{x}}_k$是后验状态估计，即经过一次修正后，当前系统状态的最优估计值；${\hat{x}}_k^{-}$为系统状态的先验估计；$z_k$为当前状态的测量样本值；${\hat{z}}_k^{-}$为当前状态的先验测量值；$z_k-{\hat{z}}_k^{-}$代表实际系统与估计系统间随机误差的影响；$K_k$权衡先验模型与实测数据间对后验分布的关系，称为卡尔曼增益，显然，$z_k-{\hat{z}}_k^{-}$越小表明实际与预测越接近，则后验分布越趋近于先验分布，$z_k-{\hat{z}}_k^{-}$越大表明预测越不可信，需要通过实测来大幅修正先验分布。

下面要确定$K_k$使后验模型的修正效果最佳，即${\hat{x}}_k$越接近真实值$x_k$。定义误差向量

$$\{\begin{array}{l}\text{后验误差向量}{e}_k=x_k-{\hat{x}}_k\\ \text{先验误差向量}{e}_k^{-}=x_k-{\hat{x}}_k^{-}\end{array}$$

定义后验误差协方差矩阵为:

$$P_k=E\left(e_k{e}_k^T\right)=\left[\begin{array}{cccc}{\sigma }_{e1}^2& {\sigma }_{e1}{\sigma }_{e2}& \cdots & {\sigma }_{e1}{\sigma }_{en}\\ {\sigma }_{e2}{\sigma }_{e1}& {\sigma }_{e2}^2& \cdots & {\sigma }_{e2}{\sigma }_{en}\\ ⋮& ⋮& \ddots & ⋮\\ {\sigma }_{en}{\sigma }_{e1}& {\sigma }_{en}{\sigma }_{e2}& \cdots & {\sigma }_{en}^2\end{array}\right]$$

同理也有先验误差协方差矩阵$P_k^{-}$。根据最小方差估计原理，设损失函数为$\mathrm{tr}\left(P_k\right)$，即要求

$$K_k=\arg \min \left[\mathrm{tr}\left(P_k\right)\right]$$

将$P_k$展开为：

上面运用了状态更新方程与状态预测方程，考虑到$e_k^{-}$与$v_k$相互独立，则进一步：

$$\begin{array}{rl}{P}_k& =\left(I-K_{k}C\right)E\left(e_k^{-}{e_k^{-}}^T\right){\left(I-K_{k}C\right)}^T+K_{k}E\left(v_k{v}_k^T\right)K_k^T\\ & =\left(I-K_{k}C\right)P_k^{-}{\left(I-K_{k}C\right)}^T+K_{k}R{K}_k^T\end{array}$$

令$\frac{\partial \mathrm{tr}\left(P_k\right)}{\partial K_k}=0$，即

$$\text{卡尔曼增益调整方程}{K}_k=\frac{P_k^{-}{C}^T}{C{P}_k^{-}{C}^T+R}$$

将$K_k$代入后验误差协方差矩阵表达式，即得

$$\text{协方差更新方程}{P}_k=\left(I-K_{k}H\right)P_k^{-}$$

要更新$K_k$，则只需要确定先验误差协方差矩阵$P_k^{-}$。同样地，将 $P_k^{-}$展开为

$$\begin{array}{rl}{P}_k^{-}& =E\left(e_k^{-}{e}_k^{-T}\right)\\ & =E\left[\left(x_k-{\hat{x}}_k^{-}\right){\left(x_k-{\hat{x}}_k^{-}\right)}^T\right]\\ & =E\left[\left(A{e}_{k-1}+w_{k-1}\right){\left(A{e}_{k-1}+w_{k-1}\right)}^T\right]\\ & =AE\left(e_{k-1}{e}_{k-1}^T\right)A^T+E\left(w_{k-1}{w}_{k-1}^T\right)\end{array}$$

考虑到$e_k$与$w_k$相互独立，进一步得到

$$\text{协方差预测方程}{P}_k^{-}=A{P}_{k-1}{A}^T+Q$$

至此，得到卡尔曼滤波的五大基本公式，其中核心方程为基于最小方差估计的卡尔曼增益调整方程，具体工作流程如图所示。

4 具体案例：船舶GPS定位

有一船舶出港沿某直线方向航行，辅助北斗卫星进行定位和测速。假设
① 船舶加速度$a\left(t\right)$=机动加速度$u\left(t\right)$+随机加速度$w\left(t\right)$，其中$w\left(t\right)$符合高斯分布；
② GPS观测噪声$v\left(t\right)$符合高斯分布。
要求用卡尔曼滤波器估计真实运动轨迹。

解决方案

首先建立随机系统的真实模型。以码头出发点为原点，采样周期(雷达扫描周期)为$T$，用$s(k)$表示船舶在采样时刻$kT$处的真实位置，用$z\left(k\right)$表示在时刻$kT$处的GPS定位观测值，则真实系统输出方程为$z(k)=s(k)+v(k)$。

记在$kT$时刻处船舶速$\dot{s}\left(k\right)$，加速度为$a\left(k\right)$，由匀加速公式有：

$$\begin{gathered} s\left(k+1\right)=s\left(k\right)+\dot{s}\left(k\right)T+\frac{1}{2}a\left(k\right)T^2 \\ \Rightarrow \dot{s}\left(k+1\right)=\dot{s}\left(k\right)+Ta\left(k\right) \end{gathered}$$

其中$a(k)=u(k)+w(k)$。

定义在采样时刻$kT$系统状态$x\left(k\right)$为船舶的位置和速度，即$x(k)={\left[\begin{array}{cc}s(k)& \dot{s}(k)\end{array}\right]}^T$可得到船舶运动的状态空间模型

$$\{\begin{array}{l}\left[\begin{array}{c}s(k+1)\\ \dot{s}(k+1)\end{array}\right]=\left[\begin{array}{cc}1& T\\ 0& 1\end{array}\right]\left[\begin{array}{c}s(k)\\ \dot{s}(k)\end{array}\right]+\left[\begin{array}{c}0.5T\\ T\end{array}\right]u(k)+\left[\begin{array}{c}0.5T\\ T\end{array}\right]w(k)\\ z(k)=\left[\begin{array}{cc}1& 0\end{array}\right]\left[\begin{array}{c}s(k)\\ \dot{s}(k)\end{array}\right]+v(k)\end{array}$$

在不考虑机动目标的动力因素即$u(k)=0$时，将匀速直线运动的船舶系统扩展到四维，状态包含水平和纵向的位置和速度，则系统方程可化为

$$\begin{gathered} \left[\begin{array}{c}x(k)\\ \dot{x}(k)\\ y(k)\\ \dot{y}(k)\end{array}\right]=\left[\begin{array}{cccc}1& T& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& T\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{c}x(k-1)\\ \dot{x}(k-1)\\ y(k-1)\\ \dot{y}(k-1)\end{array}\right]+\left[\begin{array}{cccc}0.5T^2& & & \\ & T& & \\ & & 0.5T^2& \\ & & & T\end{array}\right]\left[\begin{array}{c}{w}_1\left(k\right)\\ w_2\left(k\right)\\ w_3\left(k\right)\\ w_4\left(k\right)\end{array}\right] \\ z(k)=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 0& 1& 0\end{array}\right]\left[\begin{array}{c}x(k)\\ \dot{x}(k)\\ y(k)\\ \dot{y}(k)\end{array}\right]+\left[\begin{array}{c}{v}_1\left(k\right)\\ v_2\left(k\right)\end{array}\right] \end{gathered}$$

现假设初始位置为(-100m，200m)，水平运动初速度为2m/s，垂直运动初速度为20m/s，雷达扫描周期$T=1s$，则系统状态空间进一步化为

$$\{\begin{array}{l}x(k+1)=Ax(k)+Bu(k)+\Gamma w(k)\\ z(k)=Hx(k)+v(k)\end{array}$$

其中$A=\left[\begin{array}{cccc}1& 1& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& 1\\ 0& 0& 0& 1\end{array}\right]$，$B=\Gamma =\left[\begin{array}{cccc}0.5& & & \\ & 1& & \\ & & 0.5& \\ & & & 1\end{array}\right]$,$H=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 0& 1& 0\end{array}\right]$ 。

设过程噪声$w\sim N(0,0.1^2)$，其四个分量独立同分布，则

$$Q^{\ast }=E(w_{4\times 1}{w}_{4\times 1}^T)={\sigma }_w^2\left[\begin{array}{cccc}1& & & \\ & 1& & \\ & & 1& \\ & & & 1\end{array}\right]$$

考虑噪声矩阵$\Gamma$的加权得到过程噪声协方差矩阵

$$Q=Q^{\ast }\cdot \Gamma ={\sigma }_w^2\left[\begin{array}{cccc}0.5& & & \\ & 1& & \\ & & 0.5& \\ & & & 1\end{array}\right]$$

观测噪声$v\sim N(0,10^2)$，其两个分量独立同分布，则观测噪声协方差矩阵

$$R={\sigma }_v^2\left[\begin{array}{cc}1& \\ & 1\end{array}\right]$$

依此建立卡尔曼滤波器模型。

设置最优估计初值$\hat{x}(1)={\left[\begin{array}{cccc}-100& 2& -200& 20\end{array}\right]}^T$、先验协方差矩阵$P_1^{-}=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$，并以此开始迭代，得到实验图像如图所示。

在使用卡尔曼滤波器时，应至少保证系统模型或系统测量至少一个有足够的精度，否则卡尔曼滤波器无法从中提取到正确的估计信息。这是因为卡尔曼滤波调整的是对先验模型值与仪器测量值间的信任权重，若二者都不准确则卡尔曼跟踪的数值亦不准确。为验证这一结论，下面保持测量噪声不变，调整过程噪声方差，如图所示。

本文的完整工程代码请通过下方名片联系我获取

---

🔥 更多精彩专栏：

• 《ROS从入门到精通》
• 《Pytorch深度学习实战》
• 《机器学习强基计划》
• 《运动规划实战精讲》
• …

👇源码获取 · 技术交流 · 抱团学习 · 咨询分享 请联系👇