卡尔曼滤波

一、卡尔曼滤波思想

  将带有噪声的实际测量值mkm_k与模型估计值xkx_k进行数据融合，从而获得更精确的估计值。核心是对kkk_k进行求解。

xk=xk−1+kk(mk−xk−1)=kkmk+(1−kk)xk−1kk=eESTk−1eESTk−1+eMEAk(1.1)\begin{aligned} x_k &= x_{k-1} + k_k (m_k - x_{k - 1}) \\ &= k_k m_k + (1 - k_k) x_{k - 1} \\ k_k &= \frac{e_{EST_{k - 1}}}{e_{EST_{k - 1}} + e_{MEA_{k}}} \end{aligned} \tag{1.1}

  卡尔曼滤波只涉及：上一次的估计值xk−1x_{k-1}；当前测量值mkm_k；卡尔曼滤波系数kkk_k。kkk_k由上一次估计误差eESTk−1e_{EST_{k - 1}}与当前的测量误差eMEAk−1e_{MEA_{k - 1}}组成。

eEST=估计值−真实值eMEA=测量值−真实值(1.2)\begin{array}{l} e_{EST} = 估计值 - 真实值 \\ e_{MEA} = 测量值 - 真实值 \\ \end{array} \tag{1.2}

二、前提概念

1. 数据融合

目标：将两组测量数据合并，得到一个方差更小更精确的数据：δ\delta 小于 δ1\delta_1 和 δ2\delta_2

{m1(z1,δ1)m2(z2,δ2)=>m(z,δ)(2.1)\left \{ \begin{aligned} m_1(z_1,\delta_1) \\ m_2(z_2,\delta_2) \\ \end{aligned} => m(z,\delta) \right . \tag{2.1}

步骤：

  1. 引用卡尔曼思路：令z=z1+k(z2−z1)z = z_1 + k(z_2 - z_1)；
  2. 将上式进行方差计算：δ2(z)=var(z1+k(z2−z1))=(1−k)2δ12+(kδ2)2\delta^2(z) = var(z_1 + k(z_2 - z_1)) = (1 - k)^2 \delta_1^2 + (k \delta_2)^2
  3. 求令δ\delta最小的k：dδ2dk=0\frac{\mathbf{d} \delta^2}{\mathbf{d} k} = 0；计算出k值。

2. 协方差矩阵

概念：

  1. 方差(variance)：体现的是数据偏离期望值(均值)的程度

    δx2=1n[(x1−x‾)2+(x2−x‾)2+⋯+(xn−x‾)2](2.2)\delta^2_x = \frac{1}{n}[(x_1 - \overline{x})^2 + (x_2 - \overline{x})^2 + \dotsm +(x_n - \overline{x})^2 ] \tag{2.2}

  2. 协方差(Covariance)：衡量两个变量的总体误差
    δxy2=1n[(x1−x‾)(y2−y‾)+⋯+(xn−x‾)(yn−y‾)](2.3)\delta_{xy}^2 = \frac{1}{n}[(x_1 - \overline{x})(y_2 - \overline{y}) + \dotsm +(x_n - \overline{x})(y_n - \overline{y}) ] \tag{2.3}

  3. 协方差矩阵：囊括了多个变量的方差和协方差
    xyzxyz[δx2δxy2δxz2δyx2δy2δyz2δzx2δzy2δz2](2.4)\begin{array}{r} x \qquad y \qquad z \qquad \\ \begin{array}{l} x \\ y \\ z \\ \end{array} \begin{bmatrix} \delta_x^2 & \delta_{xy}^2 & \delta_{xz}^2 \\ \delta_{yx}^2 & \delta_y^2 & \delta_{yz}^2 \\ \delta_{zx}^2 & \delta_{zy}^2 & \delta_z^2 \end{bmatrix} \end{array} \tag{2.4}

计算：以三个变量为例

  1. 过度矩阵
    a=[x1y1z1x2y2z2x3y3z3]−13[111111111][x1y1z1x2y2z2x3y3z3]a = \begin{bmatrix} x_1 & y_1 & z_1 \\ x_2 & y_2 & z_2 \\ x_3 & y_3 & z_3 \end{bmatrix} - \frac{1}{3} \begin{bmatrix} 1 & 1 & 1 \\ 1 & 1 & 1 \\ 1 & 1 & 1 \\ \end{bmatrix} \begin{bmatrix} x_1 & y_1 & z_1 \\ x_2 & y_2 & z_2 \\ x_3 & y_3 & z_3 \end{bmatrix}
  2. 协方差矩阵：p=13aTap = \frac{1}{3}a^Ta

3. 离散的状态方程

  卡尔曼滤波只能应用于线性模型。
{xk=Axk−1+Buk+wkmk=Hxk+vk(2.5)\left \{ \begin{aligned} x_k &= A x_{k-1} + B u_k + w_{k} \\ m_k &= H x_k + v_k \end{aligned} \right . \tag{2.5}

wkw_{k}为数学模型的噪声，满足P(w)∼(0,Q)P(w) \sim (0,Q)；vkv_{k}为数学模型的噪声，满足P(v)∼(0,R)P(v) \sim (0,R)。其中Q与R为协方差矩阵。

4. 先验与后验

以状态方程为例：

  1. 先验：由于wkw_k不可测，通过模型能计算的值是先验值x^k−\hat{x}_k^-。
    x^k−=Ax^k−1+Buk(2.6)\hat{x}_k^- = A \hat{x}_{k-1} + B u_k \tag{2.6}

  2. 后验：最终通过卡尔曼滤波估计到的值x^k\hat{x}_k

三、卡尔曼滤波公式

1. 前提

2. 推导

1）问题描述

先验估计值：x^k−=Ax^k−1+Buk\hat{x}_k^- = A \hat{x}_{k-1} + B u_k
测量估计值：x^E=H−1zk\hat{x}_E=H^{-1}z_k
后验估计值：x^k=x^k−+G(H−1zk−x^k−)\hat{x}_k = \hat{x}_k^- + G(H^{-1}z_k - \hat{x}_k^-)；令G=kkHG = k_kH，得x^k=x^k−+kk(zk−Hx^k−)\hat{x}_k = \hat{x}_k^- + k_k(z_k - H\hat{x}_k^-)
目标：求出kkk_k实现ek−=x^k−xk=>0e_k^- = \hat{x}_k - x_k => 0

注意
  对于后验估计：x^k=x^k−+kk(zk−z^k−)\hat{x}_k = \hat{x}_k^- + k_k(z_k - \hat{z}_k^-), z^k−=Hx^k−\hat{z}_k^- = H\hat{x}_k^- 是通过过测量值的偏差进行数据修正的。因此就能进行模型的参数估计（参数辨识），用于外推预测。

2）目标具体化

假设eke_k满足正太分布：P(e)∼(0,Pe)P(e) \sim (0,P_e)。
问题的目标是让ek→0e_k \rightarrow 0。根据正太分布假设，就是要 tr(Pe)tr(P_e)最小。
tr(Pe)=δe12+δe22+⋯(3.1)tr(P_e) = \delta_{e1}^2 + \delta_{e2}^2 + \dotsm \tag{3.1}

3）计算 Pek=E(eeT)P_{ek} = E(ee^T)
Pek=(Pek−−kkHPek−)(I−HTkkT)+kkRkkT带入后面的kk=Pek−−kkHPek−(3.2)\begin{aligned} P_{ek} &= (P_{ek}^- - k_k H P_{ek}^-)(I - H^T k_k^T) + k_k R k_k^T \\ &带入后面的 k_k \\ &= P_{ek}^- - k_k H P_{ek}^- \end{aligned} \tag{3.2}

4）计算 dtr(Pek)dkk=0\frac{\mathbf{d}tr(P_{ek})}{\mathbf{d}k_k} = 0

kk=Pek−HTHPek−HT+R(3.3)k_k = \frac{P_{ek}^-H^T}{HP_{ek}^-H^T + R} \tag{3.3}

5）计算Pek−=E(ek−ek−T)P_{ek}^- = E(e_k^-e_k^{-T})
Pek−=APe k−1AT+Q(3.4)P_{ek}^- = A P_{e \ k-1}A^T + Q \tag{3.4}

3. 卡尔曼滤波流程

预测部分:

校正部分：

参考

卡尔曼滤波器