← 主页 胡洋的博客
← 返回博客首页

卡尔曼滤波的推导

在实际系统中，真实状态通常无法直接获得。运动模型可以预测状态如何变化，但模型总会有误差；传感器可以提供观测，但观测同样受到噪声影响。因此，状态估计的核心问题是：如何把这两类都不完全可靠的信息融合起来？

卡尔曼滤波正是为了解决这个问题。它先根据运动模型得到状态预测，再根据预测和观测各自的不确定性计算修正量，而不是简单地把传感器读数平均到状态里。本文从这个基本问题出发，推导线性 Kalman Filter（KF）的更新公式，并把结果推广到 Extended Kalman Filter（EKF）。

阅读路线： 先看第 1 节建立问题和符号；第 2 节先把一轮 KF 的完整流程走通；第 3～5 节推导 Kalman 增益；第 6 节说明 EKF 如何把非线性模型变成局部线性模型。读完本文后，可以继续阅读 ESKF 入门：从状态预测到误差注入，了解旋转状态为什么需要在误差空间中更新。

摘要

设我们要估计一个随时间变化、又无法直接观察的状态 xkx_k。运动模型告诉我们状态下一步大概会到哪里，传感器观测则提供了另一条带噪信息。Kalman Filter 将这两条信息表示为概率分布，并递归地完成两件事：

  1. 用系统模型传播状态均值和协方差，得到先验预测；
  2. 用观测残差修正预测，并得到不确定性更小的后验估计。

本文的核心推导是：假设状态更新具有形式

x^k+=x^k−+Kk(zk−Hkx^k−),\hat x_k^+ = \hat x_k^- + K_k(z_k-H_k\hat x_k^-),

其中 KkK_k 是待求的 Kalman 增益。把更新后的误差写成预测误差和观测噪声的函数，再最小化后验协方差，就能得到

Kk=Pk−Hk⊤(HkPk−Hk⊤+Rk)−1.K_k=P_k^-H_k^\top(H_kP_k^-H_k^\top+R_k)^{-1}.

EKF 的变化只有一处核心思想：当状态转移或观测模型非线性时，在当前预测点附近取一阶 Taylor 展开，用 Jacobian 代替线性模型中的 FkF_k 和 HkH_k。

关键词： Kalman Filter；KF；EKF；状态估计；协方差；Kalman 增益

1. 先把问题说清楚

1.1 我们要估计什么

在时刻 kk，xkx_k 表示需要估计的状态。它可以是一个标量，也可以是一个向量。例如：

状态通常不能被传感器直接、无噪声地读出来。我们只能利用上一时刻的估计、控制输入和当前观测，逐步逼近真实状态。

1.2 两个模型和两个噪声

离散时间的线性状态转移模型写成

xk=Fkxk−1+Bkuk+wk,x_k=F_kx_{k-1}+B_ku_k+w_k,

其中：

观测模型写成

zk=Hkxk+vk,z_k=H_kx_k+v_k,

其中：

通常假设两类噪声都是零均值高斯噪声：

wk∼N(0,Qk),vk∼N(0,Rk),w_k\sim\mathcal N(0,Q_k), \qquad v_k\sim\mathcal N(0,R_k),

并且过程噪声与观测噪声相互独立。QkQ_k 和 RkR_k 分别描述模型不确定性和传感器不确定性。

这里需要区分两个层次。后文 Kalman 增益的协方差推导，主要使用的是噪声的零均值、协方差以及独立性；即使噪声不是高斯分布，这些公式仍然给出最优线性估计器。在线性模型、初始状态为高斯分布且过程噪声和观测噪声均为高斯分布时，状态的后验分布仍然是高斯分布，此时 Kalman 更新不仅是最优线性估计，也是条件均值意义下的最小均方误差估计。实际噪声如果明显偏离高斯分布，Kalman 滤波仍可作为近似方法使用，但对异常值和重尾噪声的处理能力会下降。

1.3 预测、观测与估计的记号

本文用上标 −- 表示“融合当前观测之前”的预测值，用上标 ++ 表示“融合当前观测之后”的更新值：

x^k−和Pk−:预测状态及其协方差,\hat x_k^- \quad\text{和}\quad P_k^-: \quad\text{预测状态及其协方差},x^k+和Pk+:更新状态及其协方差.\hat x_k^+ \quad\text{和}\quad P_k^+: \quad\text{更新状态及其协方差}.

协方差不是状态本身的误差，而是误差的二阶统计量。若估计误差定义为

ek=xk−x^k,e_k=x_k-\hat x_k,

则协方差为

Pk=E[ekek⊤].P_k=\mathbb E[e_ke_k^\top].

因此，PP 越大表示估计越不确定，PP 越小表示估计越确定。

2. 一轮 Kalman Filter 做了什么

2.1 预测状态和预测协方差

假设上一时刻已经得到后验估计 (x^k−1+,Pk−1+)(\hat x_{k-1}^+,P_{k-1}^+)。先利用状态转移模型预测当前状态：

x^k−=Fkx^k−1++Bkuk.\hat x_k^-=F_k\hat x_{k-1}^+ + B_ku_k.

预测误差由上一时刻误差和过程噪声共同产生：

ek−=Fkek−1++wk.e_k^-=F_ke_{k-1}^+ + w_k.

在误差与过程噪声独立的假设下，预测协方差为

Pk−=FkPk−1+Fk⊤+Qk.P_k^-=F_kP_{k-1}^+F_k^\top+Q_k.

这一步的含义是：状态按照模型向前走，不确定性也随之传播，同时增加模型噪声带来的不确定性。

2.2 计算观测残差

根据预测状态，先计算传感器应该观测到的值：

z^k=Hkx^k−.\hat z_k=H_k\hat x_k^-.

实际观测与预测观测之差称为观测残差，也叫 innovation：

rk=zk−z^k=zk−Hkx^k−.r_k=z_k-\hat z_k=z_k-H_k\hat x_k^-.

“观测减预测”是本文的残差约定。后面的 HkH_k、Kalman 增益和状态修正都基于这个方向。

2.3 用 Kalman 增益修正状态

更新状态的形式为

x^k+=x^k−+Kkrk.\hat x_k^+=\hat x_k^-+K_kr_k.

这里 KkK_k 把观测空间中的残差转换成状态空间中的修正量。它不是凭经验选择的权重，而是由预测协方差、观测模型和观测噪声共同决定的矩阵。

更新协方差可以写成 Joseph 形式：

Pk+=(I−KkHk)Pk−(I−KkHk)⊤+KkRkKk⊤.P_k^+=(I-K_kH_k)P_k^-(I-K_kH_k)^\top+K_kR_kK_k^\top.

在理想的精确算术下，它等价于更简洁的形式：

Pk+=(I−KkHk)Pk−.P_k^+=(I-K_kH_k)P_k^-.

工程实现中，Joseph 形式通常更有利于保持协方差的对称性和半正定性。到这里，一轮 KF 可以压缩为：

预测：x^k−=Fkx^k−1++Bkuk,Pk−=FkPk−1+Fk⊤+Qk,更新：rk=zk−Hkx^k−,Kk=Pk−Hk⊤(HkPk−Hk⊤+Rk)−1,x^k+=x^k−+Kkrk.\boxed{ \begin{aligned} \text{预测：}\quad &\hat x_k^-=F_k\hat x_{k-1}^+ + B_ku_k,\\ &P_k^-=F_kP_{k-1}^+F_k^\top+Q_k,\\[2mm] \text{更新：}\quad &r_k=z_k-H_k\hat x_k^-,\\ &K_k=P_k^-H_k^\top(H_kP_k^-H_k^\top+R_k)^{-1},\\ &\hat x_k^+=\hat x_k^-+K_kr_k. \end{aligned}}

后面要做的事情，就是解释这个 KkK_k 为什么是这样的形式。

3. 先从误差看更新公式

为了避免下标干扰，下面暂时省略时间下标，记

x− 为预测状态,x+ 为更新状态.x^-\text{ 为预测状态}, \qquad x^+\text{ 为更新状态}.

3.1 假设更新结构

在线性观测模型下，我们假设更新具有如下形式：

x+=x−+K(z−Hx−).x^+=x^-+K(z-Hx^-).

这个形式很自然：保留预测状态，再根据观测残差增加一个修正量。现在唯一还没有确定的量就是 KK。

3.2 定义预测误差和更新误差

定义真实状态相对于估计状态的误差：

e−=x−x−,e+=x−x+.e^-=x-x^-, \qquad e^+=x-x^+.

观测模型为

z=Hx+v.z=Hx+v.

将更新式代入 e+e^+：

e+=x−[x−+K(z−Hx−)]=(x−x−)−K(Hx+v−Hx−)=e−−K(He−+v).\begin{aligned} e^+ &=x-\left[x^-+K(z-Hx^-)\right]\\ &=(x-x^-)-K\left(Hx+v-Hx^-\right)\\ &=e^- - K(He^-+v). \end{aligned}

于是，更新后的误差为

e+=(I−KH)e−−Kv.\boxed{e^+=(I-KH)e^- - Kv.}

这一步是整个推导的桥梁：它把更新误差写成了 KK 的函数。只要能写出更新误差的协方差，就能寻找使不确定性最小的 KK。

4. 推导更新后的协方差

4.1 展开误差的二阶矩

由协方差定义，

P+=E[e+(e+)⊤].P^+=\mathbb E[e^+(e^+)^\top].

令

A=I−KH,A=I-KH,

则 e+=Ae−−Kve^+=Ae^- - Kv。代入并展开：

P+=E[(Ae−−Kv)(Ae−−Kv)⊤]=AE[e−(e−)⊤]A⊤−AE[e−v⊤]K⊤−KE[ve−⊤]A⊤+KE[vv⊤]K⊤.\begin{aligned} P^+ ={}&\mathbb E\left[(Ae^- - Kv)(Ae^- - Kv)^\top\right]\\ ={}&A\mathbb E[e^-(e^-)^\top]A^\top -A\mathbb E[e^-v^\top]K^\top\\ &-K\mathbb E[ve^{-\top}]A^\top +K\mathbb E[vv^\top]K^\top. \end{aligned}

这里使用了常量矩阵可以移出期望的性质。

4.2 使用独立噪声假设

预测误差和当前观测噪声相互独立，因此

E[e−v⊤]=0,E[ve−⊤]=0.\mathbb E[e^-v^\top]=0, \qquad \mathbb E[ve^{-\top}]=0.

另外，

E[e−(e−)⊤]=P−,E[vv⊤]=R.\mathbb E[e^-(e^-)^\top]=P^-, \qquad \mathbb E[vv^\top]=R.

代回上式，得到

P+=(I−KH)P−(I−KH)⊤+KRK⊤.\boxed{ P^+=(I-KH)P^-(I-KH)^\top+KRK^\top.}

这就是后验协方差关于 KK 的表达式。注意它包含两种作用：KK 越大，预测误差被消除得越多；但观测噪声通过 KRK⊤KRK^\top 进入状态的程度也越大。Kalman 增益正是在这两者之间取得最优平衡。

5. 求出 Kalman 增益

5.1 把矩阵最小化变成标量最小化

严格地说，协方差是矩阵，不能直接说“让矩阵最小”。这里的目标是让后验误差在协方差意义下尽可能小。常用做法是最小化后验协方差的迹：

J(K)=tr⁡(P+).J(K)=\operatorname{tr}(P^+).

协方差的迹是各个状态分量方差之和，因此可以把它理解为总均方不确定性。对于本问题，得到的最优 KK 同时也是协方差意义下的最优线性估计增益。

将 P+P^+ 展开：

P+=P−−KHP−−P−H⊤K⊤+KHP−H⊤K⊤+KRK⊤.\begin{aligned} P^+ ={}&P^- -KHP^- -P^-H^\top K^\top\\ &+KHP^-H^\top K^\top+KRK^\top. \end{aligned}

定义观测残差的协方差

S≜HP−H⊤+R.S\triangleq HP^-H^\top+R.

SS 也叫 innovation covariance，因为它正是残差 r=z−Hx−r=z-Hx^- 的协方差：

E[rr⊤]=E[(He−+v)(He−+v)⊤]=HP−H⊤+R=S.\begin{aligned} \mathbb E[rr^\top] &=\mathbb E[(He^-+v)(He^-+v)^\top]\\ &=HP^-H^\top+R=S. \end{aligned}

利用迹的线性、循环和转置不变性，J(K)J(K) 可以写成

J(K)=tr⁡(P−)−2tr⁡(KHP−)+tr⁡(KSK⊤).J(K)=\operatorname{tr}(P^-)-2\operatorname{tr}(KHP^-)+\operatorname{tr}(KSK^\top).

5.2 对 KK 求导

由于 SS 是对称矩阵，常用的矩阵求导规则为

∂∂Ktr⁡(KA)=A⊤,\frac{\partial}{\partial K}\operatorname{tr}(KA)=A^\top,

以及

∂∂Ktr⁡(KSK⊤)=2KS.\frac{\partial}{\partial K}\operatorname{tr}(KSK^\top)=2KS.

因此，

∂J∂K=−2P−H⊤+2KS.\frac{\partial J}{\partial K} =-2P^-H^\top+2KS.

令导数为零：

KS=P−H⊤.KS=P^-H^\top.

当 SS 可逆时，得到

K=P−H⊤(HP−H⊤+R)−1.\boxed{ K=P^-H^\top(HP^-H^\top+R)^{-1}.}

这就是 Kalman 增益。

5.3 从公式理解 Kalman 增益

Kalman 增益公式中的三个部分各有明确含义：

因此：

一维例子

假设状态和观测都是标量，且观测模型为 z=x+vz=x+v，此时 H=1H=1，Kalman 增益变为

K=P−P−+R.K=\frac{P^-}{P^-+R}.

如果预测方差 P−P^- 远大于观测方差 RR，则 KK 接近 11，更新结果更靠近观测；如果观测噪声远大于预测不确定性，则 KK 接近 00，更新结果更靠近预测：

x+=(1−K)x−+Kz.x^+=(1-K)x^-+Kz.

这说明 Kalman Filter 的“加权融合”直觉是对的，但真正的权重来自协方差，而不是手工指定。

6. 从 KF 推广到 EKF

6.1 非线性状态和观测模型

现实系统通常不是线性的。非线性模型写成

xk=f(xk−1,uk)+wk,x_k=f(x_{k-1},u_k)+w_k,zk=h(xk)+vk.z_k=h(x_k)+v_k.

此时不能直接使用固定的 FF 和 HH。EKF 的做法是在当前估计附近进行一阶线性化。

6.2 对状态转移模型线性化

在上一时刻的后验估计 x^k−1+\hat x_{k-1}^+ 附近展开 ff：

f(xk−1,uk)≈f(x^k−1+,uk)+Fk(xk−1−x^k−1+),f(x_{k-1},u_k) \approx f(\hat x_{k-1}^+,u_k)+F_k(x_{k-1}-\hat x_{k-1}^+),

其中状态转移 Jacobian 为

Fk=∂f(x,uk)∂x∣x=x^k−1+.F_k=\left.\frac{\partial f(x,u_k)}{\partial x}\right|_{x=\hat x_{k-1}^+}.

因此，EKF 的预测状态和协方差写成

x^k−=f(x^k−1+,uk),\hat x_k^-=f(\hat x_{k-1}^+,u_k),Pk−=FkPk−1+Fk⊤+Qk.P_k^-=F_kP_{k-1}^+F_k^\top+Q_k.

严格来说，非线性模型中的过程噪声也可能通过 Jacobian 进入状态。若模型写成 f(x,u,w)f(x,u,w)，则还需要对 ww 求 Jacobian，得到噪声输入矩阵 GkG_k：

Pk−=FkPk−1+Fk⊤+GkQkGk⊤.P_k^-=F_kP_{k-1}^+F_k^\top+G_kQ_kG_k^\top.

6.3 对观测模型线性化

在预测状态 x^k−\hat x_k^- 附近展开观测模型：

h(xk)≈h(x^k−)+Hk(xk−x^k−),h(x_k) \approx h(\hat x_k^-)+H_k(x_k-\hat x_k^-),

其中观测 Jacobian 为

Hk=∂h(x)∂x∣x=x^k−.H_k=\left.\frac{\partial h(x)}{\partial x}\right|_{x=\hat x_k^-}.

于是 EKF 仍然使用与 KF 相同的更新结构：

rk=zk−h(x^k−),r_k=z_k-h(\hat x_k^-),Sk=HkPk−Hk⊤+Rk,S_k=H_kP_k^-H_k^\top+R_k,Kk=Pk−Hk⊤Sk−1,K_k=P_k^-H_k^\top S_k^{-1},x^k+=x^k−+Kkrk,\hat x_k^+=\hat x_k^-+K_kr_k,Pk+=(I−KkHk)Pk−(I−KkHk)⊤+KkRkKk⊤.P_k^+=(I-K_kH_k)P_k^-(I-K_kH_k)^\top+K_kR_kK_k^\top.

所以，KF 和 EKF 的差异可以概括为：

内容 KF EKF
状态转移 xk=Fkxk−1+Bkuk+wkx_k=F_kx_{k-1}+B_ku_k+w_k xk=f(xk−1,uk)+wkx_k=f(x_{k-1},u_k)+w_k
观测模型 zk=Hkxk+vkz_k=H_kx_k+v_k zk=h(xk)+vkz_k=h(x_k)+v_k
预测 Jacobian 已知的 FkF_k 在当前估计处计算 ∂f/∂x\partial f/\partial x
观测 Jacobian 已知的 HkH_k 在当前预测处计算 ∂h/∂x\partial h/\partial x
更新公式 线性模型下的 Kalman 更新 使用局部线性模型的 Kalman 更新

EKF 并不是把非线性问题精确地变成线性问题。它只在当前估计附近使用一阶近似，因此当初始误差很大、非线性很强或噪声分布明显偏离高斯时，线性化误差可能导致滤波器表现变差。

7. 一次完整推导后的检查清单

实现或阅读 KF/EKF 代码时，可以按下面的顺序检查：

  1. 状态和坐标是否定义清楚？ 明确每个状态分量的单位、坐标系和排列顺序。
  2. 残差方向是否统一？ 本文使用 r=z−z^r=z-\hat z；如果代码使用相反方向，HH 和更新公式也必须同步改变。
  3. QQ 和 RR 的含义是否混淆？ QQ 描述过程模型噪声，RR 描述观测噪声。
  4. Jacobian 的线性化点是否正确？ 预测 Jacobian 通常在上一时刻后验状态处计算，观测 Jacobian 通常在当前预测状态处计算。
  5. 矩阵维度是否匹配？ 若状态维度为 nn、观测维度为 mm，则 P∈Rn×nP\in\mathbb R^{n\times n}、H∈Rm×nH\in\mathbb R^{m\times n}、R∈Rm×mR\in\mathbb R^{m\times m}、K∈Rn×mK\in\mathbb R^{n\times m}。
  6. 是否直接求逆？ 公式写作 S−1S^{-1} 便于理解，但数值实现通常通过 Cholesky 分解或线性方程求解完成，不直接构造逆矩阵。
  7. 协方差是否保持对称？ 可以在数值误差允许范围内使用 P←(P+P⊤)/2P\leftarrow(P+P^\top)/2，并优先考虑 Joseph 形式。

对于含姿态的机器人状态，普通 EKF 的加性更新还不够，需要定义合法的旋转误差和注入方式。ESKF 将完整状态和局部误差分开维护，并在误差空间中进行 Kalman 更新，具体过程见 ESKF 入门。

8. 总结

本文沿着一条主线完成了推导：

  1. 用状态转移模型得到预测状态 x^−\hat x^- 和预测协方差 P−P^-；
  2. 用观测模型计算残差 r=z−Hx^−r=z-H\hat x^-；
  3. 从误差关系 e+=(I−KH)e−−Kve^+=(I-KH)e^- - Kv 推出后验协方差；
  4. 最小化后验不确定性，得到
K=P−H⊤(HP−H⊤+R)−1;K=P^-H^\top(HP^-H^\top+R)^{-1};
  1. 对非线性模型在当前估计附近求 Jacobian，就得到 EKF。

最值得记住的不是某一行公式，而是 Kalman Filter 的信息流：模型负责预测，观测负责约束，协方差决定两者如何融合。 KK 只是这条原则在高斯线性估计中的具体结果。

参考资料