卡尔曼滤波的公式推导和应用举例(一)
本文写于2023-08-15晚上十点
零、前言
作为机械加控制科班出身的大学生，自然是学过传统控制理论+现代控制理论的，想当年线性二阶系统的奈奎斯特图和伯德图的绘制也画的很溜。
可说起来惭愧的是，直到现在我才刚刚弄懂一点最优控制理论的一点点内容。
由于硕士期间做了计算机视觉，所以控制理论相关的内容都快忘得差不多了，好在底子还在，慢慢还能就捡了起来。
再加上学过矩阵论后，对于矩阵求导之类的不再陌生，对卡尔曼滤波的推导的接受程度也比较快，这么来看学习的每一处知识都有用。
本文的一些介绍性文字都是从相关文献或者公众号上摘抄组合而来，相关链接会放在文章末尾
一、卡尔曼滤波的介绍
1.1关于滤波
援引来自知乎大神的解释。
一位专业课的教授给我们上课的时候，曾谈到：filtering is weighting（滤波即加权）。滤波的作用就是给不同的信号分量不同的权重。最简单的loss pass filter， 就是直接把低频的信号给1权重，而给高频部分0权重。对于更复杂的滤波，比如维纳滤波, 则要根据信号的统计知识来设计权重。
从统计信号处理的角度，降噪可以看成滤波的一种。降噪的目的在于突出信号本身而抑制噪声影响。从这个角度，降噪就是给信号一个高的权重而给噪声一个低的权重。维纳滤波就是一个典型的降噪滤波器。
1.2 何为卡尔曼滤波
卡尔曼滤波（Kalman filtering）一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。
由于观测数据中包括系统中的噪声和干扰的影响，所以最优估计也可看作是滤波过程。
Kalman Filter 算法，是一种递推预测滤波算法，算法中涉及到滤波，也涉及到对下一时刻数据的预测。Kalman Filter 由一系列递归数学公式描述。它提供了一种高效可计算的方法来估计过程的状态，并使估计均方误差最小。
Kalman Filter 也可以被认为是一种数据融合算法（Data fusion algorithm），已有50多年的历史，是当今使用最重要和最常见的数据融合算法之一。Kalman Filter 的巨大成功归功于其小的计算需求，优雅的递归属性以及作为具有高斯误差统计的一维线性系统的最优估计器的状态。
1.3应用场景
卡尔曼滤波器之前被广泛用来做动态系统的状态估计、预测。主要的目的就是用来从带噪声的观测量，比如各种传感器的观测（IMU、GPS、里程计等）估计出最优的系统状态（state）。不过要明确强调的是，由于测量都带有噪声，也就是随机性，所以真正准确的状态是无法获知的。
最小二乘法可以从一长串的测量值回归出一个最为匹配的模型。卡尔曼滤波相比于最小二乘法，采用了一种递归的计算方式，也就是每一时刻只需要保存上一时刻的状态。因此可以被用来处理实时任务。
斯坦利·施密特(Stanley Schmidt)首次实现了卡尔曼滤波器。卡尔曼在NASA埃姆斯研究中心访问时，发现他的方法对于解决阿波罗计划的轨道预测很有用，后来阿波罗飞船的导航电脑使用了这种滤波器。
虽然卡尔曼滤波器从美国阿波罗登月计划到如今已有几十年，且目前有更先进的因子图（Factor Graph）算法，但是在很多领域依旧可以看到卡尔曼滤波器的身影，特别是在自动驾驶的各个模块，比如：从IMU的数据（三轴加速度，三轴角速度）计算出运动物体的当前位置。
二、基础知识准备
2.1 平均值滤波中的数据融合
假设我们对某一个物理量拿仪器进行测量k次，前k-1次的平均值为\(\bar x_{k-1}\)，第k次测量结果为\(x_k\)，则第k次时平均值滤波结果计算如下：
\[
\begin{align}
\bar x_{k} &= \frac{\bar x_{k-1} * (k - 1) + x_{k}}{k} \\
&= \bar x_{k-1} * (1 - \frac{1}{k}) + \frac{1}{k} x_{k} \\
&= \bar x_{k-1} + \frac{1}{k}(x_k - \bar x_{k-1})
\end{align}
\]
在平均值滤波中，新的\(\bar x_k\)可以看做\(x_k\)和\(\bar x_{k-1}\)的线性加权融合，\(\frac{1}{k}\)即为融合因子。\(\bar x_{k-1}\)为过往的状态值，\(x_k\)为最新的测量值，平均值滤波就可以看做是对过往的状态值和最新的状态值的融合。
将上述公式中\(\frac{1}{k}\)抽象为一个可以变化的系数\(K\)，则一般性的数据线性加权融合公式为
\[
\hat x_k = \hat x_{k-1} + K_k(z_k - \hat x_{k-1})
\]
该公式可以理解为
| 当前的估计值 = 上一次的估计值 + 系数 * (当前的测量值 - 上一次的估计值)
|
在卡尔曼滤波中，也采用了上述数据融合的思想，只不过\(K_k\)的值是随着迭代动态变化的。
2.2 卡尔曼滤波中对于数据的融合
为了描述系统特性，这里给出系统的状态方程和测量方程
\[
\begin{align}
&x_k = A_{k - 1}x_{k - 1} + B_{k-1}u_{k-1} + w_{k-1} \\
&z_k = H_k x_k + v_k
\end{align}
\]
其中，\(A_{k-1}\)为状态(转移)矩阵（Transition Matrix），\(x_{k-1}\)记为\(k-1\)时刻的状态向量。
\(u_{k-1}\)为\(k-1\)时刻输入，\(B_{k-1}\)称为控制矩阵（Control Matrix），反映了系统输入到系统状态的映射关系。
\(w_{k-1}\)是过程噪声，我们假定其符合均值为0，协方差矩阵为\(Q_{k-1}\)的高斯噪声。
\(H_k\)为测量矩阵（Measurement Matrix），描述了从系统状态到测量值的转换关系（举一个最简单的关系，系统状态是物体的直线距离，测量值是使用激光笔测出来的光从原点到物体的时间，那么\(H_k\)就是光速的倒数）
\(v_k\)是测量噪声，我们假定其符合均值为0，协方差为\(R_k\)的高斯噪声。
我们还需要引出状态预测方程和测量估计方程的概念
状态预测方程是取得是每一项的概率最大值处对应的自变量的值，噪声\(w_{k-1}\)是均值为0的高斯噪声，因此最大概率对应的值为0。
\[
\begin{align}
&\hat x^-_k = A_{k - 1}\hat x^{+}_{k - 1} + B_{k-1}u_{k-1} + 0\\
\end{align}
\]
对系统状态向量上面加上一个帽子符号表示这是一个估计量，也就是我们实际输出的量。
\(\hat x^{+}_{k - 1}\)表示\(k-1\)时刻时的卡尔曼滤波后的状态向量，也称为状态向量的后验估计。
\(\hat x^-_k\)表示当前根据系统状态方程计算预测得到的状态向量，也称为状态向量的先验估计。
测量估计方程同样忽略噪声项，变成对此时测量量的估计
\[
\hat z_k = H_k \hat x^-_k
\]
由于我们不对\(z_k\)做最优估计，所以不赋予其右上角的加减上下标。
回到卡尔曼滤波，我们要观测一个系统中的一些状态值有两种办法。
一种是通过系统的状态转移方程，并结合上一时刻的状态推得下一时刻的状态。
一种是借助辅助系统（测量系统）的测量反推出系统状态。
这两种方式都有各自的不确定性（含有噪声），卡尔曼滤波目标是将这两者做到最优结合（加权平均），使得我们估计的状态的不确定性小于其中任何一种。
卡尔曼滤波的目标是得到一个线性最小方差估计量（Linear Minimum Mean Square Error, LMMSE），使其满足该估计值与真实值之间的均方误差达到最小。
假设\(x_k\)为真实值，那么卡尔曼滤波就是找到一个合适的\(K_k\)使得下面式子中的\(\text{E}({||x_k - \hat x^+_k||}^2)\)最小。
我们根据测量方程可以反推出状态向量的测量反推值\(x_{measure}\)
\[
x^{measure}_k = H_k^{-1} z_k
\]
\[
\begin{align}
\hat x^+_k &= \hat x^-_{k} + G_k(x^{measure}_k - \hat x^-_{k}) \\
&= \hat x^-_{k} + G_k H_k^{-1} H_k (x^{measure}_k - \hat x^-_{k}) \\
&= \hat x^-_{k} + G_k H_k^{-1} (z_k - \hat z^k) \\
&= \hat x^-_{k} + K_k (z_k - \hat z^k)
\end{align}
\]
上述式子中，为了展示\(\hat x^+_k\)的融合过程，将\(K_k\)定义为\(G_k H_k^{-1}\)。
卡尔曼滤波的目标函数为
\[
minimize\ \text{E}({|x_k - \hat x^+_k|}^2) = minimize\ \text{E}({|x_k - \hat x^+_k|}*{|x_k - \hat x^+_k|}^\text{T})
\]
这里，我们尝试简化一下
\[
\begin{align}
&\text{E}({|x_k - \hat x^+_k|}*{|x_k - \hat x^+_k|}^\text{T}) = \text{D}({x_k - \hat x^+_k}) + [\text{E}({x_k - \hat x^+_k})]^2 \\
\end{align}
\]
这里\(x_k\)为真实值，表示一个常数，我们假设在初始状态下（即k=0）时，\(\hat x^+\)可以准确的描述\(x_k\)，即\(\text E(x_0 - \hat x^+_0)=0\)。
这里我们尝试化简一下\(x_k - \hat x^+_k\)
\[
\begin{align}
x_k - \hat x^+_k &= x_k - (\hat x^-_{k} + K_k (z_k - \hat z_k)) \\
&= x_k - (\hat x^-_{k} + K_k(H_k x_k + v_k - H_k \hat x^-_{k})) \\
&= x_k - \hat x^-_{k} - K_kH_k x_k - K_kv_k + K_kH_k\hat x^-_{k} \\
&= (x_k - \hat x^-_{k}) - K_kH_k(x_k - \hat x^-_{k}) - K_kv_k \\
&= (I - K_kH_k)(x_k - \hat x^-_{k}) - K_kv_k \\
&= (I - K_kH_k)[(A_{k - 1}x_{k - 1} + B_{k-1}u_{k-1} + w_{k-1}) - (A_{k - 1}\hat x^{+}_{k - 1} + B_{k-1}u_{k-1})] - K_kv_k \\
&= (I - K_kH_k)A_{k - 1}(x_{k - 1} - \hat x^{+}_{k - 1}) + (I - K_kH_k) w_{k-1} - K_kv_k \\
\end{align}
\]
\[
\begin{align}
\text{E}({x_k - \hat x^+_k}) &= E[(I - K_kH_k)A_{k - 1}(x_{k - 1} - \hat x^{+}_{k - 1}) + (I - K_kH_k) w_{k-1} - K_kv_k] \\
&= (I - K_kH_k)A_{k - 1}E(x_{k - 1} - \hat x^{+}_{k - 1})
\end{align}
\]
\[
\begin{align}
\text{E}({x_k - \hat x^+_k}) &= E[(I - K_kH_k)(x_k - \hat x^-_{k}) - K_kv_k] \\
&= (I - K_kH_k)E(x_k - \hat x^-_{k})
\end{align}
\]
在上述公式中，\(z_k\)和\(x_k\)需要理解为一个常量，而不是服从某一个分布的随机变量。
有上面的公式可以看出，对于上述线性系统的建模后，\(K_k\)取任意值，都可以保证无偏性的传递。
同时也可以看出，若后验估计保持了无偏性的传递性，先验估计\(\hat x^-_{k}\)也是无偏的。
一般的材料中，常说卡尔曼滤波的估计值是无偏估计，实际上这是由条件的，其中要求系统噪声和量测噪声为不相关、零期望的白噪声，且是线性系统，初始时刻的状态估计是无偏的。当这些条件不能满足时，Kalman滤波的估计结果是有偏的。
我们这里是先给出了卡尔曼滤波的一般公式，然后再证明其无偏性。
实际上也可以要求其无偏，设\(x^+_k = K_1 \hat x^-_{k} + K_2 x^{measure}_k\)，从无偏性中推出\(K_1\)与\(K_2\)的关系。
再回到原来的公式
\[
\begin{align}
\text{E}({|x_k - \hat x^+_k|}*{|x_k - \hat x^+_k|}^\text{T}) &= \text{D}({x_k - \hat x^+_k})
\end{align}
\]
由此可以看出，估计值与真实值之间的均方误差就是估计误差的方差。
我们继续化简
\[
\begin{align}
\text{E}({|x_k - \hat x^+_k|}*{|x_k - \hat x^+_k|}^\text{T}) &= \text{E}(\hat x^+_k - x_k)^2 \\
&= \text{E}[(\hat x^+_k - \text{E}(\hat x^+_k)) + (\text{E}(\hat x^+_k) - x_k)]^2 \\
&= \text{E}[(\hat x^+_k - \text{E}(\hat x^+_k))]^2 \\
&= \text D(\hat x^+_k)
\end{align}
\]
这里用用上了无偏估计的条件，\(\text{E}(\hat x^+_k) = x_k\)，目标函数可以化简为
\[
minimize\ \text{E}({|x_k - \hat x^+_k|}^2) = minimize\ \text{sum}(\text D(\hat x^+_k))
\]
这里又得出另外一个重要结论，在卡尔曼滤波中，最优估计值是无偏估计的条件下，最小化估计值与真实值之间的均方误差等价于最小化估计值的方差。
因此，卡尔曼滤波的思想即线性融合所有数据来源的分布，求各个分布的系数K使融合后的分布方差最小。
三、公式推导(法一)
在2.2节我们给出了卡尔曼滤波估计值公式的一般形式，也遗留了一个问题，即卡尔曼增益K如何去求。
接着2.2节末尾，我们可以得到目标函数如下：
\[
minimize\ \text D(\hat x^+_k)
\]
这里需要用到一个基础公式，\(\text{D}(\text{X}) = \text{E}(\text{X}^2) - (\text{E}\text{X})^2\) 。若\(\text{E}(\text{X}) = 0\)，则\(\text{D}(\text{X}) = \text{E}(\text{X}^2)\)
\[
\begin{align}
\hat x^+_k &= \hat x^-_{k} + K_k (z_k - \hat z^k) \\
&= \hat x^-_{k} + K_k (H_k x_k + v_k - H_k \hat x^-_k) \\
&= \hat x^-_{k} + K_k H_k( x_k - \hat x^-_k) + K_k v_k
\end{align}
\]
由于真实值\(x_k\)可以看做一个常数，不包含参数，则
\[
\begin{align}
minimize \text{E}({|x_k - \hat x^+_k|}^2) &= minimize\ \text D(\hat x^+_k) \\
&= minimize\ \text D(\hat x^-_{k} + K_k H_k( x_k - \hat x^-_k) + K_k v_k) \\
&= minimize\ \text D(x^-_{k} - x_k - K_k H_k( x_k - \hat x^-_k) + K_k v_k) \\
&= minimize\ \text D((I - K_k H_k)(\hat x^-_k - x_k) + K_k v_k)
\end{align}
\]
\[
\begin{align}
&\ D((I - K_k H_k)( \hat x^-_k - x_k) + K_k v_k) \\
&= D((I - K_k H_k)( \hat x^-_k - x_k)) + D(K_k v_k) \\
&= E((I - K_k H_k)( \hat x^-_k - x_k)( \hat x^-_k - x_k)^T(I - K_k H_k)^T) + E(K_k v_k v_k^T K_k^T ) \\
&= (I - K_k H_k)E(( \hat x^-_k - x_k )( \hat x^-_k - x_k )^T)(I - K_k H_k)^T + K_kD(v_k)K_k^T \\
&= (I - K_k H_k)E(| \hat x^-_k - x_k |^2)(I - K_k H_k)^T + K_kR_kK_k^T
\end{align}
\]
为了最小化上述公式，我们将上式定义为\(P_k^+\)（\(P_k^+ = \text{E}({| \hat x^+_k - x_k|}^2) = \text D(\hat x_k^+)\) )，用于表示状态估计误差矩阵。记\(P_k^- = E(| \hat x^-_k - x_k |^2)\)
我们定义\(k\)时刻最优状态估计的方差和，即\(\text D(\hat x_k^+)\)（\(\text{E}({|\hat x^+_k - x_k|}^2)\)）的对角线之和（因为对角线上即为各个状态分类的方差），为最小化\(J_k\)，我们有:
\[
J_k = tr(P_k^+)
\]
下面会进入实数对矩阵的求导环节，先给出几个基本公式，具体证明读者可以查阅矩阵论相关书籍进行推导
\[
\begin{align}
&\frac{\partial (ABA^T)}{\partial A} = 2AB \\
&tr(AB) = tr(BA) \\
&tr(A \pm B) = tr(A) \pm tr(B)
\end{align}
\]
设Y = AXB,已知\(\frac{\partial f}{\partial Y}\), f为将Y转换为一个标量的函数，则
\[
\begin{align}
d f &= tr((\frac{\partial f}{\partial Y})^T d Y) \\
&= tr((\frac{\partial f}{\partial Y})^T d(AXB)) \\
&= tr((\frac{\partial f}{\partial Y})^T Ad(X)B) \\
&= tr((\frac{\partial f}{\partial Y})^T Ad(X)B) \\
&= tr(B(\frac{\partial f}{\partial Y})^T Ad(X)) \\
&= tr(A^T (\frac{\partial f}{\partial Y}) B^T d(X))
\end{align}
\]
\[
\begin{align}
\frac{\partial f}{\partial X} = A^T (\frac{\partial f}{\partial Y}) B^T
\end{align}
\]
下面进入求导实操环节
\[
\begin{align}
\frac{\partial J_k}{\partial K_k} &= \frac{\partial tr((I - K_k H_k)P_k^-(I - K_k H_k)^T)} {\partial K_k} + \frac{\partial tr(K_kR_kK_k^T) }{\partial K_k} \\
&= -\frac{\partial tr((I - K_k H_k)P_k^-(I - K_k H_k)^T)} {\partial (I - K_k H_k)} H_k^T + 2K_kR_k \\
&= -2(I - K_k H_k)P_k^-H_k^T + 2K_kR_k
\end{align}
\]
令上式为0，求得
\[
K_k = P_k^- H_k^T (R_k + H_k P_k^- H_k^T)^{-1}
\]
将\(K_k\)带入到\(P_k^+\)求得
\[
\begin{align}
P_k^+ &= (I - K_k H_k)P_k^-(I - K_k H_k)^T + K_kR_kK_k^T \\
&= (I - K_k H_k)P_k^-(I - K_k H_k)^T + ((I - K_k H_k)P_k^-H_k^T)K_k^T \\
&= (I - K_k H_k)P_k^- - (I - K_k H_k)P_k^-H_k^TK_k^T + ((I - K_k H_k)P_k^-H_k^T)K_k^T \\
&= (I - K_k H_k)P_k^-
\end{align}
\]
下面再看\(P_k^-\)的计算
\[
\begin{align}
P_k^- &= E(| \hat x^-_k - x_k |^2) \\
&= E(|(A_{k - 1}\hat x^{+}_{k - 1} + B_{k-1}u_{k-1}) - (A_{k - 1} x_{k - 1} + B_{k-1}u_{k-1} + w_{k-1})|^2) \\
&= E(|A_{k - 1}(\hat x^{+}_{k - 1} - x_{k - 1}) - w_{k-1}|^2) \\
&= E((A_{k - 1}(\hat x^{+}_{k - 1} - x_{k - 1}) - w_{k-1})(A_{k - 1}(\hat x^{+}_{k - 1} - x_{k - 1}) - w_{k-1})^T) \\
&= A_{k - 1}E(|\hat x^{+}_{k - 1} - x_{k - 1}|^2)A_{k - 1}^T + E(w_{k-1} w_{k-1}^T) - E(w_{k-1})A_{k - 1}E(\hat x^{+}_{k - 1} - x_{k - 1}) - A_{k - 1}E(\hat x^{+}_{k - 1} - x_{k - 1})E(w_{k-1}) \\
&= A_{k - 1}E(|\hat x^{+}_{k - 1} - x_{k - 1}|^2)A_{k - 1}^T + E(w_{k-1} w_{k-1}^T) \\
&= A_{k - 1}P_{k-1}^{+}A_{k - 1}^T + Q_{k-1}
\end{align}
\]
至此，我们终于推导完卡尔曼滤波中的关键参数\(K_k\)和\(P_k^-\)
下面整理一下，给出卡尔曼滤波的全部公式
①状态预测方程
\[
\hat x^-_k = A_{k - 1}\hat x^{+}_{k - 1} + B_{k-1}u_{k-1}
\]
②最优状态估计迭代更新公式
\[
\hat x^+_k = \hat x^-_{k} + K_k (z_k - H_k \hat x^-_k)
\]
③卡尔曼增益计算公式
\[
K_k = P_k^- H_k^T (R_k + H_k P_k^- H_k^T)^{-1}
\]
④最优(后验)估计误差协方差更新公式
\[
P_k^+ = (I - K_k H_k)P_k^-
\]
⑤先验估计误差协方差更新公式
\[
P_k^- = A_{k - 1}P_{k-1}^{+}A_{k - 1}^T + Q_{k-1}
\]
四、结语
我在标题和第三章的地方都写了“一”这样的字样，这代表着还有二三四....，毕竟还有一些证明方法没有总结，还有一些应用案例没有举例，不过总算开了个头。
一直想总结下卡尔曼滤波相关的知识，看了网上好多资料后，决定也发表下自己的看法和证明方式。
五、参考文献和链接
- https://www.zhihu.com/question/331568328/answer/735287556
- https://zhuanlan.zhihu.com/p/134595781
- https://zhuanlan.zhihu.com/p/578920168
- https://zhuanlan.zhihu.com/p/341440139
- https://mp.weixin.qq.com/s/MQUFI6IlN6JrUz2aju7eWA
最后更新:
March 21, 2024
创建日期:
March 21, 2024