Kalman Filter原理及公式推导

August 28, 2025 作者: funnywii 分类: 算法 浏览: 81 评论: 0

卡尔曼滤波是一种高效的递归（自回归）滤波器。能够从一系列的不完全及包含噪声的测量中，估计动态系统的状态。卡尔曼滤波会根据各测量值在不同时间下的值，考虑各时间下的联合分布，再产生对未知变量的估计，因此会比只以单一测量值为基础的估计方式要准​^{[1]}。

几个值

先说明一下卡尔曼滤波中涉及到的各个值：

  • 真实值 True Value：系统状态的实际情况，是未知的。要知道就用不着KF了。如：车辆真实速度30kph。
  • 观测值 Measurement：通过传感器或其他手段获取的信息，但是会存在误差。如：速度传感器获取的速度28kph。也可以叫测量值。
  • 预测值 Prediction：根据上一时刻的估计值和系统动态模型预测到当前时刻的状态，也会存在误差。如：根据上一时刻估计的速度和加速度，预测得到当前时刻的速度。上一时刻估计值Estimation为29kph，加速度为0，那么预测车速仍为29kph。
  • 估计值 Estimation：卡尔曼滤波的输出，基于观测值Measurement和预测值Prediction的加权，目的是尽可能接近真实值True Value。如：根据观测值Measurement和预测值Prediction加权，估计值Estimation计算为28.5kph。

卡尔曼滤波本质

卡尔曼滤波相比于中值、均值等滤波器，最大的特点无延迟。使用历史数据生成一个预测值，然后使用预测值和当前测量值进行加权，加权的权重是动态更新的，最终得到估计值。

所以KF的本质是一个加权平均。

在1维情况下，两个互相独立的随机变量满足：

x_1 \sim N(\mu , \sigma_1^2) \\ x_1 \sim N(\mu , \sigma_1^2)

加权平均数为：

x = kx_1 + (1-k)x_2

卡尔曼滤波就是多维情况下的加权平均。

当着两个随机变量独立时，加权平均后的方差为：

D(x) = k^2\sigma^2_1 + (1-k)^2\sigma^2_2 \\ = (k-\cfrac{\sigma_2^2}{\sigma_2^2 + \sigma_1^2})(\sigma_2^2 + \sigma_1^2) + \cfrac{\sigma_2^2 \sigma_1^2}{\sigma_2^2 + \sigma_1^2}

为了令方差​D(x)最小，应该让权重​k为：

k = \cfrac{\sigma_2^2}{\sigma_2^2 + \sigma_1^2}

此时方差​D(x)为：

D(x) = \cfrac{\sigma_2^2 \sigma_1^2}{\sigma_2^2 + \sigma_1^2}

卡尔曼滤波示例

示例1​^{[2]}

简单来说，卡尔曼滤波就是有多个不确定的结果，经过分析、推理和计算，获得相对准确的结果。

假设一辆车，从原点出发以2m/s匀速自西向东直线运动。​t-1时刻位于原点正东6m处，​t时刻雷达测得车距离原点东9m处。已知车匀速直线运动，运动模型方差 ​\theta_q^2=4(m^2)，雷达距离方差 ​\theta_r^2=1(m^2)。问：当前小车距原点何处？或者问：小车距离原点何处可能性最大？

关于方差，一般方差的单位是测量值单位的平方。位置的单位为​m时，那么其方差的单位应为​m^2。方差的单位其实没有什么意义，而且此处方差的单位和平方米并非一个概念。

根据匀速直线运动方程，​t时刻车应距离原点以东​x_t=8m，称之为预测值。此模型并未考虑阻力和​t-1时刻速度和t-1时刻位置准确性等因素。对于雷达测量的​z_t=9m，称为观测值，由于测量仪器本身存在误差，也不完全准确。

那么如何利用预测值和观测值获取更准确的位置？一种方法是取两者均值，即二者均50%权重，结果为8.5m。但这并未利用二者的方差，方差越大代表结果可信度越低。因此方差越大的值，应该具有更低的权重。因此使用公式：

\frac{\theta^2_r}{\theta^2_r+\theta^2_q}\cdot x_t + \cfrac{\theta^2_q}{\theta^2_q + \theta^2_r} \cdot z_t = 8.8m

上述公式也是卡尔曼滤波最优值的更新公式。

示例2​^{[5]}

假设一个车辆在时刻 ​t 的位置和速度状态 ​x_t。

x_t = \begin{bmatrix} p_t \\ v_t \end{bmatrix}

其中位置​p_t

p_t=p_{t-1}+v_{t-1}\times \Delta t + u_t \times \cfrac{\Delta t^2}{2}

速度状态​v_t

v_t = v_{t-1} + u_t \times \Delta t

通过上面两个公式，可以看到 ​p_t 以及 ​v_t 是输入变量的线性组合。因此也可以写为矩阵格式。

\begin{bmatrix} p_t \\ v_t \end{bmatrix} = \begin{bmatrix} 1 & \Delta t \\ 0 & 1 \end{bmatrix} \begin{bmatrix}p_{t-1} \\ v_{t-1} \end{bmatrix} + \begin{bmatrix} {\Delta t^2}/{2} \\ \Delta t \end{bmatrix} u_t
F_t = \begin{bmatrix} 1 & \Delta t \\ 0 & 1 \end{bmatrix} , B_t = \begin{bmatrix} {\Delta t^2}/{2} \\ \Delta t \end{bmatrix}

公式可以简化为卡尔曼滤波中第一个公式，即状态预测公式：

\hat{x}^-_t = F_t \hat{x}_{t-1} + B_tu_t

其中 ​F_t 为状态转移矩阵，表示如何从上一时刻 ​t-1 的状态来推测当前时刻的状态，​B_t 为控制矩阵，表示控制量 ​u 如何作用于当前状态。上述 ​x 的 ​\hat {} 表示这是一个估计值，​^- 上标是 ​x 的上标而非 ​t 的，表示这个估计值是根据上一时刻的状态推测而来。在后面会根据观测量来修正 ​\hat{x}^-_t，然后才能得到最优估计值。

在KF中，不确定性用状态协方差矩阵 ​P 表示，为了让不确定性在时刻间传递，使用 ​P_t^- = FP_{t-1}F^T 来表示，即两边乘以状态转移矩阵 ​F。这是根据协方差矩阵的性质： ​cov(Ax, Bx) = Acov(x,x)B^T 得来的。为了表示预测模型本身的噪声，在后面再加入一个状态转移协方差矩阵 ​Q，即

P_t^- = FP_{t-1}F^T +Q

这也是卡尔曼滤波的第二个公式，表示不确定性在各个时刻间的传递。前两个公式是通过上一时刻的状态来预测当前时刻的状态。

然后，假设我们使用一个观测器，观测这个汽车的位置 ​z_t，即观测值。从汽车本身状态 ​x_t 到 ​z_t，需要一个观测矩阵 ​H 来进行线性变换。

在这里，​x_t 和 ​z_t 的维度可以不同，​z_t 的观测值只有位置即1个维度，但是 ​x_t 包含了两个维度，因此可以假设 ​H = [1 \quad 0]，当 ​x_t 和矩阵 ​H 相乘可以得到标量 ​z_t。可以得到公式 ​z_t=Hx_t +v，由于观测值也存在误差，因此引入了观测噪声 ​v，其协方差矩阵用观测噪声方差矩阵 ​R 来表示。

根据上面得到的 ​\hat{x}^-_t，得到最优估计值公式（第四个公式，第三个公式在后面）：

\hat{x}_t = \hat{x}^-_t + K_t(z_t - H \hat{x}^-_t)

公式中 ​z_t - H \hat{x}^-_t 表示实际观测值和预测的观测值的残差。

残差是指观察值与模型估计值之间的差

​K_t 为卡尔曼增益，其作用是权衡 预测状态协方差 ​P 和 观察值协方差 ​R 的大小来决定预测模型与观察模型的权重。以及把残差的表现形式从观察域转换到状态域。

卡尔曼增益的计算公式（第三个公式）为：

K_t=P_t^-H^T(HP_t^-H^T + R)^{-1}

最后，更新最佳估计值的噪声分布（第五个公式），留给下一轮迭代使用：

P_t = (I-K_tH)P_t^-

状态的不确定性是减小的，下一轮迭代中由于传递噪声的引入，不确定性又会增大。KF就是在不确定性变化中寻找平衡。

原理推导

（线性）卡尔曼滤波的应用基于以下三个假设前提：

  • 当前时刻状态只和上一时刻状态有关。
  • 模型和系统均满足线性关系。
  • 引入的噪声符合高斯分布。

对于和多时刻状态有关、非线性、非高斯问题，不能简单地使用卡尔曼滤波。

接下来开始推导公式，在本小节中，时间用 ​k 表示。

状态向量

状态向量 ​x 用来描述系统状态，KF的目的就是估计 ​x 。

如果为 ​x 加上一个 ​x^{-} ，则代表先验的预测结果。

状态方程

这个概念来自控制理论，不过多展开，主要用于描述上一时刻 ​k-1 的状态到当前时刻 ​k 的状态向量的变化是如何发生的。

x_k=Fx_{k-1}+Bu_{k}+w_k \tag 1

其中：

  • ​x_k 表示 ​k 时刻真实值，即待估计值。如位置、速度等。
  • ​F 表示状态转移矩阵。
  • ​x_{k-1} 表示 ​k-1 时刻的真实值。
  • ​B 表示控制矩阵。
  • ​u_{k} 表示 ​k 时刻的控制输入量，如加速度。
  • ​w_k 表示过程噪声，均值0，协方差 ​Q_k 的高斯分布。

状态预测方程

根据状态方程，去掉噪声可以得出状态预测方程。

\hat{x}^-_k = F\hat{x}_{k-1} + Bu_k \tag 2

这里 ​\hat{x} 表示的是估计的量（与准确的量相对），如果没有这个hat，可以认为是一个准确的表达式，也就是上面状态方程的表达形式。

其中：

  • ​\hat{x}^-_k 表示当前 ​k 时刻的预测值。
  • ​\hat{x}_{k-1} 表示 ​k-1 时刻的最优估计值（后验）。

状态更新方程

根据卡尔曼滤波本质那节的内容，我们希望构造出一个线性组合形式的加权和：

\hat{x}_k = \alpha \hat{x}^-_k + \beta z_k

但是更合理的结构应该是：

\hat{x}_k = \hat{x}_k^- + K_k (z_k - H \hat{x}_k^-) \tag 3

这个形式的含义，残差项代表测量值和预测的测量值的残差，也就是说测量和预测一致，那么残差为0，估计值完全信任预测值，否则根据残差修正，其中：

  • ​\hat{x}_k 表示 ​k 时刻的最优估计值。
  • ​K_k 表示卡尔曼增益。
  • ​\hat{x}_k^- 表示 ​k 时刻的预测值。
  • ​z_k 表示 ​k 时刻的测量值。

如果是一维状态空间，也就是标量形式，观测矩阵为 ​I ，那么就有如下形式：

\hat{x}_k = K_kz_k+(1-K_k)\hat{x}^-_k

可以看出，更新方程本质就是一个加权和。

测量方程

之所以引入测量方程，是因为测量的值，和要估计的状态向量可能不一一对应。比如测量值只有速度，但是要状态向量却是位置和速度，这就要引入一个矩阵，把状态向量投影到测量空间。

z_k = Hx_k + v_k \tag 4

其中：

  • ​H 为测量矩阵。
  • ​v_k 为测量噪声，均值为0，协方差为 ​R 的高斯分布。

e.g. 一个表示位置和速度的状态向量，但是我们的测量值仅有速度。

x = \begin{bmatrix} pos\\ velo\\ \end{bmatrix}

此时使用一个测量矩阵​H：

H = \begin{bmatrix} 0 & 1\\ \end{bmatrix}
Hx= \begin{bmatrix} 0 & 1\\ \end{bmatrix} \begin{bmatrix} pos\\ velo\\ \end{bmatrix} = \begin{bmatrix} 0\\ velo\\ \end{bmatrix} = z

也可以说，​H 仅让测量部分得到修正，不会影响其他状态变量。

如果忽略噪声，可以得到：

\hat{z}_k = H \hat{x}^-_k \tag 5

完整示例

这里给出一个完整的示例，可以对状态向量、状态和测量方程有更直观的理解。当然这里不包含卡尔曼增益的更新和公式推导。

比如我们要估计一个车辆的位置和速度，假设其匀速运动。

那么状态向量 ​x_{t-1} 应为：

x_{t-1} = \begin{bmatrix} x_{t-1}\\ v_{t-1}\\ \end{bmatrix}

状态转移矩阵 ​F 应为：

F = \begin{bmatrix} 1 & t\\ 0 & 1\\ \end{bmatrix}

那么，​Fx 项的结果就是：

Fx_{t-1} = \begin{bmatrix} x_{t-1} + v_{t-1}\times t\\ v_{t-1}\\ \end{bmatrix}

符合运动学公式。

那么如果有了一个加速度 ​a ，比如刹车，对系统产生了一个外界的力，那么需要用 ​u_k 来代表这个力。

此时，控制向量 ​u 为：

u_t = [a]

控制矩阵 ​B 为：

B = \begin{bmatrix} \frac{1}{2}t^2\\t \\ \end{bmatrix}

那么 ​Bu 的结果为：

Bu = \begin{bmatrix} \frac{1}{2}at^2\\at \\ \end{bmatrix}

合起来 ​Fx_{x-1}+Bu_t 就是：

\begin{bmatrix} x_t\\ v\\ \end{bmatrix} = \begin{bmatrix} x_{t-1} + v_{t-1}t + \frac{1}{2}t^2\\ v_{t-1} + at\\ \end{bmatrix}

喏，完整的运动学公式。可以看出，当无外力的时候，也就是惯性运动，状态向量​x 完全由 ​F 决定；当存在外力，需要引入 ​B 建模。

卡尔曼增益计算

K_k = \cfrac{P^-_kH^t}{HP^-_kH^t+R} \tag 6

这是卡尔曼增益计算的公式，其中：

  • ​P_k 是预测误差的协方差矩阵。
  • ​R 是测量噪声的协方差矩阵。

先不管上面两个噪声协方差矩阵怎么来的，我们先推导卡尔曼增益。

卡尔曼增益的推导

先看一眼公式2，是使用卡尔曼增益作为权重，计算当前测量值和上一时刻最优估计的加权平均和，以得到当前时刻的最优估计值。那么我们肯定是希望当前时刻最优估计值和真实值的误差最小。那么就有目标函数：

e_k = x_k - \hat{x}_k \rarr min_k |e_k|

将上面公式4带入公式3，两边同时使用 ​x_k 减，以构造 ​e_k ，在这部分 ​K_k 简写为 ​K（手敲真的好累...）：

x_k - \hat{x}_k = x_k -x^-_k -K(Hx_k+v_k - H\hat{x}^-_k)

得到：

\begin{aligned} e_k &= (1-KH)(x_k-\hat{x}^-_k)-Kv_k \\ & =(1-KH)e^-_k-Kv_k \end{aligned} \tag 7

在处理随机变量和最优目标函数的问题时，直接通过随机变量的关系式来求解最优目标函数比较费劲，因此通过数学期望来表征随机变量的特征可以简化求解流程：

P_k=E[e_ke^t_k] \qquad P^-_k = E[e^-_ke^{-t}_k]

上面两个数学期望分别代表真实值与估计值的后验误差协方差，以及真实值与预测值的先验误差协方差。

此处有个结论：​P_k=P_k^t

对于公式7，两边分别乘自己的自己的转置，在左侧构造数学期望：

\begin{aligned} E[e_ke^t_k] &=E[((I-KH)e^-_k-Kv_k)((I-KH)e^-_k-Kv_k)^t \\ &= E[(I-KH)e^-_ke^{-t}_k(I-KH)^t - (I-KH)e^-_kv^t_kK^t - Kv_ke^{-t}_k(I-KH)^t + Kv_kv_k^tK^t] \end{aligned}

其中，由于 ​v_k 与 ​\hat{x}_k \quad x^-_k \quad x_k 无关，所以 ​v_k 与 ​e_k \quad e^-_k 相互独立，相互独立变量的数学期望有​E[AB] = E[A]E[B]。

又因为 ​v_k 满足均值0的高斯分布，那么​E[v_k]=0 ，所以上式中的 ​(I-K_kH)e^-_kv^t_kK^t 和 ​K_kv_ke^{-t}_k(I-K_kH)^t 这两项的期望都为0。

那么有：

\begin{aligned} P_k &= (I-KH)P^-_k(I-H^tK^t) + KE[v_kv^t_k]K^t \\ &= (I-KH)P^-_k(I-H^tK^t) + KRK^t \end{aligned}

展开后得到：

P_k = P_k^- -KHP^-_k - P^-_kH^tK^t + K(HP^-_kH^t + R)K^t \tag 8

基于上述公式8，对 ​P_k 取迹 ​tr，并对 ​K_k 求导，可以获得使 ​tr(P_k) 最小的 ​K_k 值。

之所以这样做，基于以下3点：

  1. ​tr(P_k) = 0 \rarr e_k = 0
  2. ​tr(P_k)为标量函数，且凸，必有最小值
  3. ​tr(P_k) 对 ​K_k 求导满足求导乘积法则

此外，关于迹还有几点结论，不做证明：

\begin{aligned} \frac{\partial tr(AX)}{\partial X} &= A^T \\ \frac{\partial tr(XA)}{\partial X} &= A^T \\ \frac{\partial tr(XAX^T)}{\partial X} &= (AX^T + A^TX^T)^T &= X(A + A^T) \\ \frac{\partial tr(X^TAX)}{\partial X} &= (X^TA + X^TA^T)^T &= (A + A^T)X\\ \frac{\partial tr(P)}{\partial X} &= \frac{\partial tr(P^T)}{\partial X} \end{aligned}

根据以上结论，对公式8取迹后进行求导：

\begin{aligned} \frac{\partial tr(P_k)}{\partial K} &= \frac{\partial tr(P_k^-)}{\partial K} - \frac{\partial tr(KHP_k^-)}{\partial K} - \frac{\partial tr(P_k^-H^tK^t)}{\partial K} + \frac{\partial tr(K(HP_k^-H^t + R)K^t)}{\partial K} \\ &= 0 - (HP_k^-)^t - (HP_k^-)^t + K[(HP_k^-H^t + R) + (HP_k^-H^t + R)^t] \\ &= -2P_k^-H^t + 2K(HP_k^-H^t + R) \\ &= 0 \end{aligned}

可以得到上面的公式6：

K = P_k^- H^t (H P_k^- H^t + R)^{-1} \tag 6

预测误差协方差的推导

将公式6带入到公式8，并削去一些项可以得到：

P_k = P_k^- - KHP_k^- - P_k^-H^tK^t + P_k^-H^tK^t = (I - KH)P_k^-

根据状态预测方程即公式2，为了构造 ​e^-_k ，公式2两侧同时使用​x_k^-去减：

\begin{aligned} x_k - \hat{x}_k^- &= x_k - F\hat{x}_{k-1} - Bu_{k} \\ &= Fx_{k-1} + Bu_{k} + w_k - F\hat{x}_{k-1} - Bu_{k} \\ &= F(x_{k-1} - \hat{x}^-_{k-1}) + w_k \end{aligned}

有：

e_k^- = Fe^-_k + w_k

再同时乘自己的转置以构造 ​P^-_k：

\begin{aligned} E[e_k^-e_k^{-t}] &= E[(Fe_{k-1} + w_k)(Fe_{k-1} + w_k)^t] \\ &= E[Fe_{k-1}e_{k-1}^tF^t + w_kw_k^t] \end{aligned}

和 ​v_k 一样，​w_k 同样是均值 0 的高斯分布，因此可以得到：

P_k^- = FP_{k-1}F^t + Q \qquad \tag 9

预测噪声R和测量噪声Q协方差

在实际应用中，​Q和​R的值需要通过多次试验和调整来确定。

卡尔曼滤波黄金五条公式

预测

\hat{x}_k^-= F \hat{x}_{k-1} + Bu_k \tag a
P_k^- = FP_{k-1}F^T +Q \tag b

更新

K_k = \cfrac{P_k^-H^T}{HP^-_kH^T +R} \tag c
\hat{x}_k = \hat{x}^-_k + K_k(z_k - H\hat{x}^-_k) \tag d
P_k = (I-K_kH)P^-_k \tag e

参考文章

[1] 卡尔曼滤波 - 维基百科，自由的百科全书

[2] 深入浅出理解卡尔曼滤波【实例、公式、代码和图】 - 知乎

[3] Kalman滤波通俗理解+实际应用_卡尔曼滤波应用实例-CSDN博客

[4] 【卡尔曼滤波器】5_直观理解与二维实例【包含完整的EXCEL代码】_哔哩哔哩_bilibili

[5] 卡尔曼滤波器的原理以及在matlab中的实现_哔哩哔哩_bilibili

[6] 卡尔曼滤波 Kalman Filter 之美在于什么？ - 知乎

#Algorithm(48)

评论