卡尔曼滤波

卡尔曼滤波总结与思考

这两天开始N+1刷DR_CAN的视频，刚学过现代控制理论，又被可以收敛的全阶观测器震撼了一波，寻思挑战一下原本一头雾水的卡尔曼滤波，看完推导过程稍微有点头绪，还没来得及做实验，先总结一下理论部分。

系统模型

过程模型：

x_k=Ax_{k-1} + Bu_{k-1} + w_{k-1}

测量模型：

z_k=Hx_{k-1}+v_k

卡尔曼滤波模型

预测部分
先验状态估计：

\hat{x}_k^-=A\hat{x}_{k-1}+Bu_{k-1} \tag{1}

先验误差协方差矩阵：

P_k^- = AP_{k-1}A^T +Q \tag{2}

更新部分
卡尔曼增益：

K_k=\frac{P_k^- H^T}{H P_k^- H^T + R} \tag{3}

后验估计：

\hat{x}_k=\hat{x}_k^- + K_k(Z_k - H \hat{x}_k^-) \tag{4}

更新误差协方差矩阵：

P_k = (I-K_k H)P_k^- \tag{5}

过程推导

上述便是卡尔曼滤波的系统模型和观测器模型，卡尔曼滤波虽叫“滤波器”但其本质上是一个观测器，而且也是是一个最优化、递归的数字处理算法，其本质是为了解决在测量系统中存在的不确定性，而系统的不确定性主要来自：

不存在完美的数学模型（控制系统建模无法绝对精准）
系统扰动不可控而且难以建模
测量过程中传感器存在误差

在现代控制理论中我们使用全阶观测器对系统的状态进行观测，并且使用反馈矫正的思想使观测器的误差不断减小，其与状态反馈控制在数学上使等价的，因此我们可以采用极点配置的方法确定观测增益 $L$ ，在工程实践中往往观测器的极点要比控制器小4~5倍，然而当极点配置过小时，虽然观测系统响应更加迅速，但是会增加观测噪声，影响最终的控制品质，所以要平衡控制器和观测器的配置，提高控制品质。

而卡尔曼观测器和上述全阶观测器的区别就是在于如何选取状态观测增益：状态观测器通过极点配置设计观测器增益，而卡尔曼滤波器通过结合量测噪声与与系统噪声估计误差的方差确定卡尔曼增益。

协方差

为了量测噪声，我们在系统模型中引入了过程噪声 $w$ 和测量噪声 $v$ ，这两个参数属于先验知识，需要我们通过估计给出，过程噪声主要反应系统模型的不确定性，测量噪声则是反应测量传感器的不确定性，但是两者都符合高斯分布，即：

w \sim P(0,Q)

v \sim P(0,R)

已知方差 $Var(x)=E(x^2)-E^2(x)$ ，由于 $E(w)=E(v)=0$
所以：

Var(w)=E(w^2)

Var(v)=E(v^2)

而 $wv$ 都是矩阵，所以 $Q$ 、 $R$ 分别为两者的协方差，两者的期望都为0，即：

Q=E[ww^T]

R=E[vv^T]

如果写成矩阵的形式（假设为二阶矩阵）：

Q= \begin{bmatrix} \sigma_{w1}^2 & \sigma_{w1}\sigma_{w2} \\ \sigma_{w2}\sigma_{w1} & \sigma_{w2}^2 \end{bmatrix}

而且协方差矩阵是对称正定矩阵，即 $\sigma_{w1}\sigma{_{w2}}=\sigma_{w2}\sigma{_{w1}}$ ，而两侧的值表示两个噪声的关联程度，如果两个噪声是相互独立的，则 $\sigma_{w1}\sigma{_{w2}}=\sigma_{w2}\sigma{_{w1}}=0$ 。

我们知道，当一组数据的方差越小，其越接近期望值，所以我们也采用协方差的方式来表述系统预测值与真实值的差值，则k时刻的误差值为：

e_k = x_k - \hat{x}_k

当然 $e$ 也符合期望为0，协方差为 $P_k$ 高斯分布：

e \sim P(0,P_k)

但是注意这个差值我们无法算出具体值，因为我们无法得知具体的系统真值 $x_k$ ，但是我们可以表示它的协方差:

P_k=E(ee^T)

只要找到某种条件使得此时的误差方差最小，就能使系统收敛，而这个过程正是确定卡尔曼增益 $K_k$ 的过程。

数据融合

但在具体推导之前我们再补充关于数据融合的知识，如果你会计算平均值的话，那么你一定熟悉这个公式：

\hat{x}_k=\frac{1}{k}(z_1+z_2+z_3+\dots+z_k)

$\hat{x}_k$ 是测量 $k$ 次后的平均值，如果我们对公式进行简单变换，便可得到当前平均值和上次平均值的关系：

\begin{aligned} \hat{x}_k &=\frac{1}{k}(z_1+z_2+z_3\dots+z_k) \\ &=\frac{1}{k}\frac{k-1}{k-1}(z_1+z_2+\dots+z_{k-1})+\frac{1}{k}z_k \\ &=\frac{k-1}{k}\hat{x}_{k-1}+\frac{1}{k}z_k \\ &=\hat{x}_{k-1}+\frac{1}{k}(z_k-\hat{x}_{k-1}) \end{aligned}

从上式不难看出：

当前预测值=上次预测值+系数（当前测量值-上次预测值）

$k$ 就是我们测量的次数，在测量初期和后期会呈现不同的规律

当 $k\to0$ 时（ $k$ 很小时）， $\hat{x}_k\to z_k$ ，预测结果越接近测量值
当 $k \to \infty$ 时， $\hat{x}_k \to \hat{x}_{k-1}$ ，预测结果越接近于上次预测值

而系数 $\frac{1}{k}$ 在卡尔曼滤波中通常用 $K_k$ 表示，意为卡尔曼增益（ $Kalman Gain$ ），现在我们引入上述的两个误差，估计误差方差和测量误差方差：

估计误差方差：e_{EST} =Var( x_k-\hat{x}_k) \\ 测量误差方差：e_{MEA} =Var(z_k-x_k)

估计误差是我们观测器测量出的误差和真实值的误差（P）
测量误差是由传感器测量过程中的误差®

引入卡尔曼增益：

K_k=\frac{e_{EST_{k-1}}}{e_{EST_{k-1}}+e_{MEA_k}}

这里的卡尔曼增益和最前面推导完的卡尔曼增益是等价的（假设测量值和状态值不需要变换，即观测矩阵H为单位阵）：

K_k=\frac{P_k^-}{P_k^- + R}

这里的 $P_{k}^-=P_{k-1}$ 即估计噪声（过程噪声）的先验误差协方差矩阵，或是上次噪声的误差协方差矩阵，他们在一维状态下是等价的。

类比上面的过程，当 $e_{EST_{k-1}}>>E_{MEA_k}$ 时， $K_k \to 1$ ， $\hat{x}_k \to z_k$ ，即测量误差远小于估计误差时，新预测值越接近测量值；而当 $e_{MEA_k}>>e_{EST_{k-1}}$ 时， $K_k \to 0$ ， $\hat{x}_k \to \hat{x}_{k-1}$ ，即估计误差远小于测量误差时，新预测值越接近模型预测值。于是我们便可以有了一个简单的预测更新算法：

K_k=\frac{e_{EST_{k-1}}}{e_{EST_{k-1}}+e_{MEA_k}} \tag{1}

\hat{x}_k=\hat{x}_{k-1}+K_k(z_k-\hat{x}_{k-1}) \tag{2}

e_{EST_k}=(1-K_k)e_{EST_{k-1}} \tag{3}

而公式（3）也是标准卡尔曼滤波公式中的误差协方差更新项简化版，同时也是用方差 $e$ 代替了高维状态下的协方差 $P$ ，并将我们计算得出的最优卡尔曼增益带入得出：

P_k = (I-K_k)P_k^-

上述公式可以看作低维、理想情况下简化的卡尔曼滤波。

以上便是将模型预测值和实际测量值进行融合，最终得出一个更加准确的观测值，下面我们来讨论如何将两个不同的传感器测量的值融合在一起，我们假设有两个不同的传感器 $z_1、z_2$ ，他们的测量值都符合高斯分布，我们假设 $z_1$ 的均值 $\mu_1$ 为30，标准差 $\sigma_1$ 为2， $z_2$ 的均值 $\mu_2$ 为32，标准差 $\sigma_1$ 为4：

z_1 \sim P(30,2^2) \\ z_2 \sim P(32,4^2) \\

如何用最优的策略融合这两个传感器的测量值呢，我们不妨用上述的融合公式：

\hat{z}=z_1 +K_k(z_2-z_1)

要使融合后的预测值为最优我们就要使 $\hat{z}$ 的方差最小，关键就是如何确定增益值，这也是卡尔曼滤波器最核心的思想，求解观测器增益使得观测值方差/协方差最小。下面计算两者融合后的预测值的方差：

Var(z_1)=\sigma_1^2 \\ Var(z_2)=\sigma_2^2

我们假设两个传感器相互独立，各自测量值不会互相影响：

\begin{aligned} Var(\hat{z})&=Var[z_1+K_k(z_2-z_1)] \\ &=Var[(1-K_k)z_1 + K_kz_2] \quad \text{(两项相互独立)} \\ &=Var[(1-K_k)z_1]+Var(K_kz_2) \quad \text{(独立可拆分)}\\ &=(1-K_k)^2Var(z_1)+K_k^2Var(z_2) \quad \text{(提出系数项)} \\ \end{aligned}

为了使估计值 $\hat{z}$ 最准确，我们需要选择 $K_k$ 使得 $Var(\hat{z})$ 最小，这个过程与下面卡尔曼增益推导是一个原理，只不过扩展到多维后计算更加复杂。

为了求解最小值，我们可以对 $\sigma_{\hat{z}}^2$ 关于 $K_k$ 求导，并使其导数等于0，找到其极值点：

\frac{d}{dK_k}Var(\hat{z})=\frac{d(1-K_k)^2\sigma_1^2+K_k^2\sigma_2^2}{dK_k}=0

展开整理得：

\begin{aligned} \frac{d\sigma_{\hat{z}}^2}{dK_k}&=\frac{d}{dK_k}(\sigma_1^2-2K_k\sigma_1^2+K_k^2\sigma_1^2+K_k^2\sigma_2^2) \\ &=\frac{d}{dK_k}[\sigma_1^2-2K_k\sigma_1^2+K_k^2(\sigma_1^2+\sigma_2^2)] \\ &=-2\sigma_1^2+2K_k(\sigma_1^2+\sigma_2^2)\\ &=0 \end{aligned}

通过求导最小化估计方差，我们得到最优增益 $K_k$ ：

K_k=\frac{\sigma_1^2}{\sigma_1^2+\sigma_2^2}

如果 $\sigma_1^2 \gg \sigma_2^2$ （即 $z_1$ 的误差远大于 $z_2$ 的误差），则 $K_k \approx 1$ ，说明更信任测量值 $z_2$ 。
如果 $\sigma_2^2 \gg \sigma_1^2$ （即 $z_2$ 的误差远大于 $z_1$ 的误差），则 $K_k \approx 0$ ，说明更信任预测值 $z_1$ 。

这样我们便完成了两个传感器的最优数据融合，而标准卡尔曼滤波就是以最优的增益融合数学模型推导的预测值 $\hat{x}_k$ 和测量值 $z_k$ ，其对应的协方差矩阵分别是 $P_k$ 和 $R$ ，其中R是我们根据传感器特性给定的协方差矩阵，这个值是我们针对不同的传感器给的数，而 $P_k$ 是我们在观测过程中不断优化，不断减小的误差协方差矩阵，我们将计算出的最优增益代入到误差协方差矩阵的计算便完成了对其的更新。

标准卡尔曼增益推导

根据上面的思想我们可以扩展到标准卡尔曼滤波中 $K_k$ 的求取，具体的代入推导暂时省略，可以去DR_CAN的视频具体看，最后我们得到了误差的协方差矩阵表达式（可以将 $P_k$ 和 $R$ 与上边的 $\sigma_1^2$ 和 $\sigma_2^2$ 等效比较一下，只不过在标准的卡尔曼滤波中不在是一维的方差而是多维的协方差矩阵）：

我们令估计误差为：

e_k=x_k-\hat{x}_k

则我们用 $P_k$ 来表示协方差矩阵：

\begin{aligned} P_k&=E(ee^T)\\ &=E[(x_k-\hat{x}_k)(x_k-\hat{x}_k)^T] \end{aligned}

让我们代入过程模型和测量模型到估计模型，计算 $e=x_k-\hat{x}_k$

\begin{aligned} x_k-\hat{x}_k&=x_k-[\hat{x}_k^- + K_k(z_k-H\hat{x}_k^-)] \\ &=x_k-\hat{x}_k^- -K_kz_k-K_kH\hat{x}_k^- \quad \text{(代入$z_k$)}\\ &=x_k-\hat{x}_k^- -K_k(Hx_k+v_k)-K_k \\ &=(x_k-\hat{x}_k^-)-K_kH(x_k-\hat{x}_k^-)-K_kv_k\\ &=(I-K_kH)(x_k-\hat{x}_k^-)-K_kv_k \quad (e_k^-=x_k-\hat{x_k^-})\\ &=(I-K_kH)e_k^--K_kv_k \end{aligned}

最后我们得出估计误差协方差矩阵 $P_k$ ：

P_k=P_k^- -K_kHP_k^- -P_k^-H^TK_k^T + K_kHP_k^-H^TK_k^T +K_kRK_k^T

于是我们也可以利用刚才对 $K_k$ 求导等于0的思想去求最优的卡尔曼增益，但是这里需要注意的是， $P_k$ 是协方差矩阵：

P_k= \begin{bmatrix} \sigma_{e1}^2 & \sigma_{e1}\sigma_{e2} \\ \sigma_{e2}\sigma_{e1} & \sigma_{e2}^2 \end{bmatrix}

真正需要降低的误差方差只是对角线上的方差，而两者之间的方差我们无需在意，这时候我们便可以利用矩阵的迹（对角线元素相加）来求解误差方差：

tr(P_k)=\sigma_{e_1}^2+\sigma_{e_2}^2 \\ =tr(P_k^-)-2tr(K_kHP_k^-)+tr(K_kHP_k^-H^TK_k^T)+tr(K_kRK_k^T)

同样我们可以对 $tr(P_k)$ 关于 $K_k$ 求导：

\frac{d}{dK_k}tr(P_k)=0

最后化简为：

\frac{d}{dK_k}tr(P_k)=-P_kH^T+K_k(HP_k^-H^T+R)=0

得出最优卡尔曼增益：

K_k=\frac{P_k^-H^T}{HP_k^-H^T+R}

求先验误差协方差矩阵

观察上述卡尔曼增益公式，我们发现现在先验误差协方差矩阵 $P_k^-$ 还是未知，但根据定义我们可知：

P_k^-=E(e_k^- e_k^{-T})

先验误差 $e_k^-$ :

\begin{aligned} e_k^-&=x_k-\hat{x}_k^- \\ &=Ax_{k-1}+Bu_{k-1}+w_{k-1}-A\hat{x}_{k-1}-Bu_{k-1} \\ &=A(x_{k-1}-\hat{x}_{k-1})+w_{k-1}\\ &=A(e_{k-1})+w_{k-1} \end{aligned}

注意，现在先验误差出现了 $w_{k-1}$ ，是在过程模型中引入的过程噪声（模型不精确、物理环境干扰），所以 $P_k^-$ 会引入过程噪声协方差矩阵 $Q$ 。

下面继续推导，将其带入到 $P_k^-$ ，得：

\begin{aligned} P_k^-&=E[(Ae_{k-1}+w_{k-1})(Ae_{k-1}+w_{k-1})^T] \\ &=E[(Ae_{k-1}+w_{k-1})(e_{k-1}^TA^T+w_{k-1}^T)] \\ &=E(Ae_{k-1}e_{k-1}^TA^T)+E(w_{k-1}e_{k-1}^TA^T) \\ &+E(Ae_{k-1}w_{k-1}^T)+E(w_{k-1}w_{k-1}^T)\\ &=AE(e_{k-1}e_{k-1}^T)A^T+E(w_{k-1}w_{k-1}^T)\\ &=AP_{k-1}A^T+Q \end{aligned}

上式中，由于 $w_{k-1}$ 与 $e_{k-1}$ 是相互独立的，且二者的期望均为0，所以两项直接消除，剩下的两项凑成了上次误差协方差 $P_{k-1}$ 和过程噪声协方差 $Q$ 。

公式小结

至此，我们便大致的推导出了卡尔曼滤波器的五个核心公式：

预测部分

先验状态估计

\hat{x}_k^-=A\hat{x}_{k-1}+Bu_{k-1} \tag{1}

首先我们利用系统模型建立了大致的数学模型来估计先验值，这一项要和过程模型区分开，过程模型是我们抽象出来的真实系统的真实值，它是再有过程噪声 $w$ 的，由于我们并无法对噪声进行建模，所以在先验状态估计中只是利用能建模的数学模型做理论值的推导

先验误差协方差矩阵

P_k^- = AP_{k-1}A^T +Q \tag{2}

先验误差协方差矩阵是我们最后推导出来的，但是它是在预测阶段的，因为它用到了上次的误差协方差矩阵，所以在初始阶段需要我们给这个 $P_0$ 赋予初始值，之后便可以不断迭代更新卡尔曼增益 $K_k$ ，因为我们可以看到，卡尔曼增益更新主要是由先验误差协方差确定的，而不断迭代的增益也进一步影响了下次协方差矩阵的更新，使其能够不断迭代优化到误差最小的状态；因为先验误差计算过程中 $e_K^-=x_k-\hat{x}_k^-$ 中含有真实过程模型中的的 $x_k$ ，其是带有过程噪声 $w$ 的，因此最后有其协方差矩阵Q，当然这里的Q阵是卡尔曼滤波过程中需要调节的重要参数之一。

更新部分

卡尔曼增益

K_k=\frac{P_k^- H^T}{H P_k^- H^T + R} \tag{3}

首先我们一开始在均值滤波过程中发现了滤波增益，其次在数据融合过程中我们通过对方差关于滤波增益求导找到了最优增益，发现滤波增益的大小在调节这所要融合的两个数据的权值，在两个传感器融合中， $K_k$ 调节的是两个传感器的权重，而在标准的卡尔曼滤波中 $K_k$ 调节的理论模型预测值和测量值之间的权重，由于引入了测量值，所以其包含测量过程中产生的噪声协方差矩阵R，R也是我们调节卡尔曼滤波中的一个重要的参数，不过我们可以通过研究我们的测量设备大概确定这个参数。