卡尔曼滤波从零到精通

本文系统讲解卡尔曼滤波(KF)与扩展卡尔曼滤波(EKF)原理及实现。涵盖KF五步递推算法(状态预测、协方差预测、卡尔曼增益计算、状态校正、协方差更新),Q与R的物理意义及调参方法,以及EKF如何通过雅可比矩阵对非线性系统进行一阶线性化。重点强调协方差传播机制、最优增益推导逻辑及其在嵌入式系统中的工程落地要点。


【全文大纲】 : 主页目录


一、为什么需要卡尔曼滤波

系统面对两个天然敌人:

  1. 模型不完美:任何数学模型都是近似的,有未建模动态、参数误差、离散化误差。
  2. 测量不完美:传感器有噪声,读到的不是真值。

只信模型,误差会累积漂移。只信测量,噪声会让结果抖动。

卡尔曼滤波的核心思想是:用模型预测,用测量修正,按谁更可信自动分配权重。

这个权重就是卡尔曼增益 K,它由预测协方差 P 减和测量噪声 R 自动算出来。


二、温度例子

要估计两个量:

  • 温度 T,单位摄氏度
  • 升温速率 V,单位摄氏度每秒

状态向量:

x = [ T V ] x=\begin{bmatrix}T\\V\end{bmatrix} x=[TV​]

模型假设:

T k = T k − 1 + V k − 1 Δ t T_k=T_{k-1}+V_{k-1}\Delta t Tk​=Tk−1​+Vk−1​Δt

V k = V k − 1 V_k=V_{k-1} Vk​=Vk−1​

测量只测温度:

z k = T k + v k z_k=T_k+v_k zk​=Tk​+vk​

其中 v 是测量噪声。


三、符号系统全解

这一节把每个符号一次性讲清楚。所有公式的推导都建立在这些符号的定义上。

状态 x

x = [ T V ] x=\begin{bmatrix}T\\V\end{bmatrix} x=[TV​]

  • T 是温度估计值,单位摄氏度。
  • V 是升温速率估计值,单位摄氏度每秒。
  • 为什么用两个状态:只估温度无法跟踪趋势,加上速率才能对斜坡变化有预测能力。
  • 为什么是向量:状态可以有多个分量,统一用矩阵运算。

状态转移矩阵 F

F = [ 1 Δ t 0 1 ] F=\begin{bmatrix}1&\Delta t\\0&1\end{bmatrix} F=[10​Δt1​]

  • Δt 是两次采样的时间间隔,单位秒。
  • F 描述上一时刻状态怎么变到当前时刻。
  • 来源:把模型写成矩阵形式。

[ T k V k ] = [ 1 Δ t 0 1 ] [ T k − 1 V k − 1 ] \begin{bmatrix}T_k\\V_k\end{bmatrix}= \begin{bmatrix}1&\Delta t\\0&1\end{bmatrix} \begin{bmatrix}T_{k-1}\\V_{k-1}\end{bmatrix} [Tk​Vk​​]=[10​Δt1​][Tk−1​Vk−1​​]

  • 物理意义:温度按当前速率外推,速率保持不变,即匀速假设。

观测矩阵 H

H = [ 1 0 ] H=\begin{bmatrix}1&0\end{bmatrix} H=[1​0​]

  • 为什么存在:状态是二维,测量是一维,需要一个矩阵把状态投影到测量空间。
  • 为什么是 1 和 0:因为只测温度,不测速率。

z = H x + v = [ 1 0 ] [ T V ] + v = T + v z=Hx+v= \begin{bmatrix}1&0\end{bmatrix} \begin{bmatrix}T\\V\end{bmatrix}+v=T+v z=Hx+v=[1​0​][TV​]+v=T+v

  • 物理意义:H 回答测的是什么。

测量 z 与观测方程

z = H x + v z=Hx+v z=Hx+v

  • z 是传感器读数,标量,只测温度。
  • v 是测量噪声,标量。
  • 为什么存在:测量是真实世界的反馈,用来把预测拉回来。

过程噪声 w 与协方差 Q

x k = F x k − 1 + w x_k=Fx_{k-1}+w xk​=Fxk−1​+w

  • w 是过程噪声,表示模型不完美。
  • Q 是过程噪声的协方差矩阵,定义为:

Q = E [ w w T ] Q=E[ww^T] Q=E[wwT]

温度模型里,假设速率被一个随机加速度 a 扰动:

w = [ 1 2 a Δ t 2 a Δ t ] w=\begin{bmatrix}\frac{1}{2}a\Delta t^2\\a\Delta t\end{bmatrix} w=[21​aΔt2aΔt​]

Q = E [ w w T ] = q [ Δ t 4 / 4 Δ t 3 / 2 Δ t 3 / 2 Δ t 2 ] Q=E[ww^T]=q \begin{bmatrix} \Delta t^4/4 & \Delta t^3/2\\ \Delta t^3/2 & \Delta t^2 \end{bmatrix} Q=E[wwT]=q[Δt4/4Δt3/2​Δt3/2Δt2​]

其中:

q = E [ a 2 ] q=E[a^2] q=E[a2]

  • q 是加速度噪声强度。
  • 为什么出现 Δt 平方和 Δt 四次方除以 4:加速度积分一次得速率噪声,积分两次得温度噪声,平方后就是这两个系数。
  • 物理意义:Q 表示模型本身有多不靠谱。

测量噪声 v 与协方差 R

z = H x + v z=Hx+v z=Hx+v

  • v 是传感器噪声,标量。
  • R 是测量噪声方差:

R = E [ v 2 ] R=E[v^2] R=E[v2]

代码里 R 等于 0.04,对应标准差 0.2 摄氏度。

  • 物理意义:R 表示传感器有多不准。

协方差 P 与上标负号 P 减

P = E [ e e T ] P=E[ee^T] P=E[eeT]

其中:

e = x true − x ^ e=x_{\text{true}}-\hat x e=xtrue​−x^

  • e 是状态估计误差。
  • P 是误差的协方差矩阵。

P = [ P 00 P 01 P 10 P 11 ] P= \begin{bmatrix} P_{00} & P_{01}\\ P_{10} & P_{11} \end{bmatrix} P=[P00​P10​​P01​P11​​]

元素含义
P00温度误差的方差
P11速率误差的方差
P01 等于 P10温度与速率误差的协方差
  • P 减:上标负号表示预测阶段,还没用测量校正。
  • P 无负号:校正阶段,用了测量之后。

P 不是状态,是对估计有多不确定。

卡尔曼增益 K

K = [ K 0 K 1 ] K=\begin{bmatrix}K_0\\K_1\end{bmatrix} K=[K0​K1​​]

  • K0 是温度校正增益。
  • K1 是速率校正增益。
  • 为什么是 2 乘 1:新息是标量,状态是二维,需要把标量按比例分配到两个状态。

新息 innovation

innovation = z − H x ^ − \text{innovation}=z-H\hat x^- innovation=z−Hx^−

  • H 乘 x 减:根据预测,测量应该是什么。
  • z 减 H 乘 x 减:测量和预测的差,叫新息。
  • 物理意义:测量告诉你预测错了多少。

四、卡尔曼滤波五步推导

第一步:状态预测

x ^ − = F x ^ \hat x^-=F\hat x x^−=Fx^

代码:

T_pred = kf->T + kf->V * dt;
V_pred = kf->V;

展开:

T pred = T + V Δ t T_{\text{pred}}=T+V\Delta t Tpred​=T+VΔt

V pred = V V_{\text{pred}}=V Vpred​=V

推导:直接代入状态转移矩阵。

含义:按当前速率外推温度,速率保持不变。

输入:上一轮校正后的 T 和 V。

输出:预测状态 T 预测值和 V 预测值。

第二步:协方差预测

P − = F P F T + Q P^-=FPF^T+Q P−=FPFT+Q

代码:

P00p = P00 + dt*(P01+P10) + dt*dt*P11 + Q00;
P01p = P01 + dt*P11 + Q01;
P10p = P10 + dt*P11 + Q01;
P11p = P11 + Q11;

推导第 1 步,误差传播。

真实状态:

x − = F x + w x^-=Fx+w x−=Fx+w

预测状态:

x ^ − = F x ^ \hat x^-=F\hat x x^−=Fx^

预测误差:

e − = x − − x ^ − = F ( x − x ^ ) + w = F e + w e^-=x^--\hat x^-=F(x-\hat x)+w=Fe+w e−=x−−x^−=F(x−x^)+w=Fe+w

推导第 2 步,代入协方差定义。

P − = E [ e − ( e − ) T ] = E [ ( F e + w ) ( F e + w ) T ] P^-=E[e^-(e^-)^T]=E[(Fe+w)(Fe+w)^T] P−=E[e−(e−)T]=E[(Fe+w)(Fe+w)T]

展开:

P − = E [ F e e T F T ] + E [ F e w T ] + E [ w e T F T ] + E [ w w T ] P^-=E[Fee^TF^T]+E[Few^T]+E[we^TF^T]+E[ww^T] P−=E[FeeTFT]+E[FewT]+E[weTFT]+E[wwT]

推导第 3 步,利用期望的线性性质。w 和 e 不相关,交叉项为 0。

E [ F e w T ] = 0 , E [ w e T F T ] = 0 E[Few^T]=0,\quad E[we^TF^T]=0 E[FewT]=0,E[weTFT]=0

第一项:

E [ F e e T F T ] = F   E [ e e T ]   F T = F P F T E[Fee^TF^T]=F\,E[ee^T]\,F^T=FPF^T E[FeeTFT]=FE[eeT]FT=FPFT

第四项:

E [ w w T ] = Q E[ww^T]=Q E[wwT]=Q

推导第 4 步,得结果。

P − = F P F T + Q P^-=FPF^T+Q P−=FPFT+Q

为什么左边是 F,右边是 F 转置:左边误差变成 F 乘 e,所以左边出现 F。右边误差的转置变成 e 转置乘 F 转置,所以右边出现 F 转置。缺一不可。

物理意义:

  • FPF 转置:旧不确定度经过状态转移传播。
  • Q:模型噪声新增的不确定度。

第二步是卡尔曼滤波的灵魂,它算外推之后有多不确定。

输入:上一轮 P,已知 Q,已知 F。

输出:预测协方差 P 减。

第三步:卡尔曼增益

K = P − H T ( H P − H T + R ) − 1 K=P^-H^T(HP^-H^T+R)^{-1} K=P−HT(HP−HT+R)−1

代码:

S = P00p + kf->R;
K0 = P00p / S;
K1 = P10p / S;

推导:温度滤波器里 H 是 1 和 0,所以:

H P − H T = P 00 − HP^-H^T=P_{00}^- HP−HT=P00−​

P − H T = [ P 00 − P 10 − ] P^-H^T=\begin{bmatrix}P_{00}^-\\P_{10}^-\end{bmatrix} P−HT=[P00−​P10−​​]

代入得:

K = [ P 00 − / ( P 00 − + R ) P 10 − / ( P 00 − + R ) ] = [ K 0 K 1 ] K= \begin{bmatrix} P_{00}^-/(P_{00}^-+R)\\ P_{10}^-/(P_{00}^-+R) \end{bmatrix} =\begin{bmatrix}K_0\\K_1\end{bmatrix} K=[P00−​/(P00−​+R)P10−​/(P00−​+R)​]=[K0​K1​​]

K 为什么长这样:K 是让校正后协方差最小的最优值。

一维推导:

校正后误差:

e = ( 1 − K ) e − − K v e=(1-K)e^--Kv e=(1−K)e−−Kv

校正后方差:

P = ( 1 − K ) 2 P − + K 2 R P=(1-K)^2P^-+K^2R P=(1−K)2P−+K2R

对 K 求导,令导数为 0:

d P d K = − 2 ( 1 − K ) P − + 2 K R = 0 \frac{dP}{dK}=-2(1-K)P^-+2KR=0 dKdP​=−2(1−K)P−+2KR=0

解得:

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

K 是让校正后方差最小的最优权重,不是拍脑袋选的。

物理意义:

K 0 = P 00 − P 00 − + R K_0=\frac{P_{00}^-}{P_{00}^-+R} K0​=P00−​+RP00−​​

  • P00 减大:预测温度方差大,预测不可信,K0 大,更信测量。
  • R 大:测量噪声大,测量不可信,K0 小,更信预测。

K 1 = P 10 − P 00 − + R K_1=\frac{P_{10}^-}{P_{00}^-+R} K1​=P00−​+RP10−​​

  • P10 减:温度与速率的误差协方差。
  • 相关性越强,温度残差里速率信息越多,K1 越大,越用温度残差修速率。

K0 管温度,K1 管速率,都来自 P 减和 R 的拔河。

输入:P 减,R,H。

输出:K。

第四步:状态校正

x ^ = x ^ − + K ( z − H x ^ − ) \hat x=\hat x^-+K(z-H\hat x^-) x^=x^−+K(z−Hx^−)

代码:

innovation = z - T_pred;
kf->T = T_pred + K0 * innovation;
kf->V = V_pred + K1 * innovation;

展开:

T = T pred + K 0 ( z − T pred ) T=T_{\text{pred}}+K_0(z-T_{\text{pred}}) T=Tpred​+K0​(z−Tpred​)

V = V pred + K 1 ( z − T pred ) V=V_{\text{pred}}+K_1(z-T_{\text{pred}}) V=Vpred​+K1​(z−Tpred​)

含义:最终估计等于预测值加上增益乘测量残差。

  • x 减:先猜一个。
  • z 减 H 乘 x 减:测量说猜错了多少。
  • K:错多少信几成。
  • x:修正后的最终估计。

为什么是 z 减 H 乘 x 减而不是 z 减 x 减:状态二维,测量一维,维度不同。H 把状态投影到测量空间。

H x ^ − = [ 1 0 ] [ T pred V pred ] = T pred H\hat x^-=\begin{bmatrix}1&0\end{bmatrix}\begin{bmatrix}T_{\text{pred}}\\V_{\text{pred}}\end{bmatrix}=T_{\text{pred}} Hx^−=[1​0​][Tpred​Vpred​​]=Tpred​

H 的作用:把状态变成测量应该是什么,才能和测量比较。

二维时 K 如何分配:

  • K0:残差修温度。
  • K1:残差修速率。
  • 同一个残差,按不同比例分给两个状态。

输入:x 减,K,z。

输出:校正后状态 T 和 V。

第五步:协方差更新

P = ( I − K H ) P − P=(I-KH)P^- P=(I−KH)P−

代码:

P00 = P00p - K0 * P00p;
P01 = P01p - K0 * P01p;
P10 = P10p - K1 * P00p;
P11 = P11p - K1 * P01p;

推导第 1 步,校正公式。

x ^ = x ^ − + K ( z − H x ^ − ) \hat x=\hat x^-+K(z-H\hat x^-) x^=x^−+K(z−Hx^−)

推导第 2 步,代入观测方程。

z = H x + v z=Hx+v z=Hx+v

得:

x ^ = x ^ − + K ( H x + v − H x ^ − ) \hat x=\hat x^-+K(Hx+v-H\hat x^-) x^=x^−+K(Hx+v−Hx^−)

推导第 3 步,定义误差,推误差传播。

e = x − x ^ e=x-\hat x e=x−x^

e = x − x ^ − − K ( H e − + v ) = e − − K H e − − K v e=x-\hat x^--K(He^-+v)=e^--KHe^--Kv e=x−x^−−K(He−+v)=e−−KHe−−Kv

提取 e 减:

e = ( I − K H ) e − − K v e=(I-KH)e^--Kv e=(I−KH)e−−Kv

推导第 4 步,取协方差。

P = E [ e e T ] P=E[ee^T] P=E[eeT]

P = E [ ( ( I − K H ) e − − K v ) ( ( I − K H ) e − − K v ) T ] P=E[((I-KH)e^--Kv)((I-KH)e^--Kv)^T] P=E[((I−KH)e−−Kv)((I−KH)e−−Kv)T]

假设 e 减和 v 不相关:

P = ( I − K H ) P − ( I − K H ) T + K R K T P=(I-KH)P^-(I-KH)^T+KRK^T P=(I−KH)P−(I−KH)T+KRKT

这是完整协方差更新公式。

推导第 5 步,最优 K 化简。在最优 K 下可以证明:

( I − K H ) P − ( I − K H ) T + K R K T = ( I − K H ) P − (I-KH)P^-(I-KH)^T+KRK^T=(I-KH)P^- (I−KH)P−(I−KH)T+KRKT=(I−KH)P−

所以:

P = ( I − K H ) P − P=(I-KH)P^- P=(I−KH)P−

完整公式是两项之和,最优 K 下化简为 I 减 KH 乘 P 减。

为什么用 I 不用 1:e 减是向量,KH 是矩阵,提取公因子必须用单位矩阵。

e − − K H e − = ( I − K H ) e − e^--KHe^-=(I-KH)e^- e−−KHe−=(I−KH)e−

I 等于:

I = [ 1 0 0 1 ] I=\begin{bmatrix}1&0\\0&1\end{bmatrix} I=[10​01​]

I 是矩阵版本的 1,保证 I 乘 e 减等于 e 减。

展开四行代码:

K H = [ K 0 0 K 1 0 ] KH=\begin{bmatrix}K_0&0\\K_1&0\end{bmatrix} KH=[K0​K1​​00​]

I − K H = [ 1 − K 0 0 − K 1 1 ] I-KH=\begin{bmatrix}1-K_0&0\\-K_1&1\end{bmatrix} I−KH=[1−K0​−K1​​01​]

逐元素乘:

P 00 = P 00 − − K 0 P 00 − P_{00}=P_{00}^--K_0P_{00}^- P00​=P00−​−K0​P00−​

P 01 = P 01 − − K 0 P 01 − P_{01}=P_{01}^--K_0P_{01}^- P01​=P01−​−K0​P01−​

P 10 = P 10 − − K 1 P 00 − P_{10}=P_{10}^--K_1P_{00}^- P10​=P10−​−K1​P00−​

P 11 = P 11 − − K 1 P 01 − P_{11}=P_{11}^--K_1P_{01}^- P11​=P11−​−K1​P01−​

为什么 P 一定比 P 减小:

P 00 = P 00 − ( 1 − K 0 ) P_{00}=P_{00}^-(1-K_0) P00​=P00−​(1−K0​)

因为 0 小于 K0 小于 1,所以 1 减 K0 小于 1,所以 P00 小于 P00 减。

用了测量,信息变多,不确定度一定下降。

对称化:

float cross = 0.5f * (kf->P01 + kf->P10);
kf->P01 = cross;
kf->P10 = cross;

对称化是数值修正,不是算法必需。

输入:P 减,K,H。

输出:校正后协方差 P,作为下一轮第二步的输入。


五、Q 和 R 到底是什么

Q 的推导

温度模型里,假设速率被随机加速度 a 扰动:

V k = V k − 1 + a Δ t V_k=V_{k-1}+a\Delta t Vk​=Vk−1​+aΔt

T k = T k − 1 + V k − 1 Δ t + 1 2 a Δ t 2 T_k=T_{k-1}+V_{k-1}\Delta t+\frac{1}{2}a\Delta t^2 Tk​=Tk−1​+Vk−1​Δt+21​aΔt2

与卡尔曼模型相减:

w = [ 1 2 a Δ t 2 a Δ t ] w=\begin{bmatrix}\frac{1}{2}a\Delta t^2\\a\Delta t\end{bmatrix} w=[21​aΔt2aΔt​]

取协方差:

Q = E [ w w T ] = E [ a 2 ] [ Δ t 4 / 4 Δ t 3 / 2 Δ t 3 / 2 Δ t 2 ] = q [ Δ t 4 / 4 Δ t 3 / 2 Δ t 3 / 2 Δ t 2 ] Q=E[ww^T]=E[a^2] \begin{bmatrix} \Delta t^4/4 & \Delta t^3/2\\ \Delta t^3/2 & \Delta t^2 \end{bmatrix} =q \begin{bmatrix} \Delta t^4/4 & \Delta t^3/2\\ \Delta t^3/2 & \Delta t^2 \end{bmatrix} Q=E[wwT]=E[a2][Δt4/4Δt3/2​Δt3/2Δt2​]=q[Δt4/4Δt3/2​Δt3/2Δt2​]

其中:

q = E [ a 2 ] q=E[a^2] q=E[a2]

w 是真实模型减卡尔曼模型的差,Q 是 w 的协方差。

q 怎么调

  • q 大:速率变化剧烈,模型不靠谱,P 减涨得快,K 大,更信测量。
  • q 小:速率基本不变,模型很准,P 减涨得慢,K 小,更信模型。

q 靠实测调,不是算出来的。

R 是什么

R = E [ v 2 ] R=E[v^2] R=E[v2]

  • 传感器噪声方差。
  • 标准 KF 里初始化后不变。
  • 代码里 R 等于 0.04,对应标准差 0.2 摄氏度。

R 是传感器有多不准,Q 是模型有多不靠谱。


六、五步闭环总览

x ^ , P → x ^ − → P − → K → x ^ → P \hat x,P \rightarrow \hat x^- \rightarrow P^- \rightarrow K \rightarrow \hat x \rightarrow P x^,P→x^−→P−→K→x^→P

步骤公式回答的问题
第一步x 减等于 F 乘 x状态应该变成多少
第二步P 减等于 F P F 转置加 Q这个预测有多不确定
第三步K 等于 P 减 H 转置乘括号逆预测和测量各信多少
第四步x 等于 x 减加 K 乘新息最终状态是多少
第五步P 等于 I 减 K H 乘 P 减校正后还剩多少不确定度

状态和协方差是两个并行的递推,协方差驱动增益,增益驱动校正。

P 减决定 K,K 决定校正,校正又更新 P。


七、从 KF 升级到 EKF

为什么需要 EKF

标准 KF 要求状态转移和观测都是线性的:

x k = F x k − 1 + w x_k=Fx_{k-1}+w xk​=Fxk−1​+w

z k = H x k + v z_k=Hx_k+v zk​=Hxk​+v

实际系统常常是非线性的:

x k = f ( x k − 1 , u k − 1 ) + w x_k=f(x_{k-1},u_{k-1})+w xk​=f(xk−1​,uk−1​)+w

z k = h ( x k ) + v z_k=h(x_k)+v zk​=h(xk​)+v

比如:

  • 电机转矩是乘积形式。
  • 机器人位置涉及三角函数。
  • 电池 SOC 和电压非线性。
  • 温度散热和温度四次方成正比。

这时不能用常数 F 和 H,要用雅可比矩阵。

EKF 的核心思想

用一阶泰勒展开,把非线性函数在当前位置线性化。

f ( x ) ≈ f ( x ^ ) + F ( x − x ^ ) f(x)\approx f(\hat x)+F(x-\hat x) f(x)≈f(x^)+F(x−x^)

h ( x ) ≈ h ( x ^ ) + H ( x − x ^ ) h(x)\approx h(\hat x)+H(x-\hat x) h(x)≈h(x^)+H(x−x^)

其中:

F = ∂ f ∂ x ∣ x ^ F=\left.\frac{\partial f}{\partial x}\right|_{\hat x} F=∂x∂f​ ​x^​

H = ∂ h ∂ x ∣ x ^ H=\left.\frac{\partial h}{\partial x}\right|_{\hat x} H=∂x∂h​ ​x^​

F 和 H 不再是常数矩阵,而是每轮在当前估计处重新算的雅可比。

EKF 五步

第一步,状态预测。

x ^ − = f ( x ^ , u ) \hat x^-=f(\hat x,u) x^−=f(x^,u)

第二步,协方差预测。形式不变,F 用雅可比。

P − = F P F T + Q P^-=FPF^T+Q P−=FPFT+Q

第三步,卡尔曼增益。形式不变,H 用雅可比。

K = P − H T ( H P − H T + R ) − 1 K=P^-H^T(HP^-H^T+R)^{-1} K=P−HT(HP−HT+R)−1

第四步,状态校正。新息变成 z 减 h 乘 x 减。

x ^ = x ^ − + K ( z − h ( x ^ − ) ) \hat x=\hat x^-+K(z-h(\hat x^-)) x^=x^−+K(z−h(x^−))

第五步,协方差更新。形式不变,H 用雅可比。

P = ( I − K H ) P − P=(I-KH)P^- P=(I−KH)P−


八、雅可比矩阵:EKF 的灵魂

雅可比是什么

对向量函数:

f ( x ) = [ f 1 ( x 1 , … , x n ) ⋮ f m ( x 1 , … , x n ) ] f(x)=\begin{bmatrix}f_1(x_1,\dots,x_n)\\\vdots\\f_m(x_1,\dots,x_n)\end{bmatrix} f(x)= ​f1​(x1​,…,xn​)⋮fm​(x1​,…,xn​)​ ​

雅可比矩阵定义为:

F = ∂ f ∂ x = [ ∂ f 1 ∂ x 1 ⋯ ∂ f 1 ∂ x n ⋮ ⋱ ⋮ ∂ f m ∂ x 1 ⋯ ∂ f m ∂ x n ] F=\frac{\partial f}{\partial x}= \begin{bmatrix} \frac{\partial f_1}{\partial x_1}&\cdots&\frac{\partial f_1}{\partial x_n}\\ \vdots&\ddots&\vdots\\ \frac{\partial f_m}{\partial x_1}&\cdots&\frac{\partial f_m}{\partial x_n} \end{bmatrix} F=∂x∂f​= ​∂x1​∂f1​​⋮∂x1​∂fm​​​⋯⋱⋯​∂xn​∂f1​​⋮∂xn​∂fm​​​ ​

物理意义:雅可比就是非线性函数在当前点的局部线性斜率。

一维函数 y 等于 f 乘 x,雅可比就是 df 除以 dx。

二维函数:

F = [ ∂ f 1 ∂ x 1 ∂ f 1 ∂ x 2 ∂ f 2 ∂ x 1 ∂ f 2 ∂ x 2 ] F=\begin{bmatrix} \frac{\partial f_1}{\partial x_1}&\frac{\partial f_1}{\partial x_2}\\ \frac{\partial f_2}{\partial x_1}&\frac{\partial f_2}{\partial x_2} \end{bmatrix} F=[∂x1​∂f1​​∂x1​∂f2​​​∂x2​∂f1​​∂x2​∂f2​​​]

具体例子

非线性模型:

x k = [ x 1 + a x 2 2 x 2 ] x_k=\begin{bmatrix}x_1+a x_2^2\\x_2\end{bmatrix} xk​=[x1​+ax22​x2​​]

雅可比:

F = [ 1 2 a x 2 0 1 ] F=\begin{bmatrix} 1&2ax_2\\ 0&1 \end{bmatrix} F=[10​2ax2​1​]

注意:雅可比里有 x2,说明它随状态变化,每轮都要重新算。

线性系统里 F 是常数,非线性系统里 F 是状态的函数。

温度例子看 EKF

真实温度模型含散热项:

T k = T k − 1 + V k − 1 Δ t − k ( T k − 1 − T env ) 4 Δ t T_k=T_{k-1}+V_{k-1}\Delta t-k(T_{k-1}-T_{\text{env}})^4\Delta t Tk​=Tk−1​+Vk−1​Δt−k(Tk−1​−Tenv​)4Δt

V k = V k − 1 V_k=V_{k-1} Vk​=Vk−1​

非线性函数:

f ( x ) = [ T + V Δ t − k ( T − T env ) 4 Δ t V ] f(x)=\begin{bmatrix}T+V\Delta t-k(T-T_{\text{env}})^4\Delta t\\V\end{bmatrix} f(x)=[T+VΔt−k(T−Tenv​)4ΔtV​]

雅可比:

F k = [ 1 − 4 k ( T − T env ) 3 Δ t Δ t 0 1 ] F_k= \begin{bmatrix} 1-4k(T-T_{\text{env}})^3\Delta t&\Delta t\\ 0&1 \end{bmatrix} Fk​=[1−4k(T−Tenv​)3Δt0​Δt1​]

KF 的 F 是常数,EKF 的 F 里有 T,每轮不同。

EKF 的 F 是随状态变化的 F。

为什么一阶展开就够

f ( x ) ≈ f ( x ^ ) + F ( x − x ^ ) f(x)\approx f(\hat x)+F(x-\hat x) f(x)≈f(x^)+F(x−x^)

  • 一阶展开在估计附近足够精确。
  • 状态方差小时,高阶项可以忽略。
  • 状态方差大或非线性强时,EKF 可能发散。

EKF 用计算量和精度,换来了处理非线性系统的能力。


九、EKF 与 KF 对照

步骤KFEKF
状态预测x 减等于 F 乘 xx 减等于 f 乘 x 和 u
协方差预测P 减等于 F P F 转置加 Q同形式,F 用雅可比
增益K 等于 P 减 H 转置乘括号逆同形式,H 用雅可比
状态校正x 等于 x 减加 K 乘新息x 等于 x 减加 K 乘 z 减 h 乘 x 减
协方差更新P 等于 I 减 K H 乘 P 减同形式,H 用雅可比

五步结构完全相同,变的只有三处:F 换成 F 雅可比,H 换成 H 雅可比,H 乘 x 换成 h 乘 x。


十、哪些东西被非线性函数替代

KF 里的东西EKF 换成什么说明
F 乘 xf 乘 x 和 u状态转移从矩阵乘法换成非线性函数
FF 雅可比,即 f 对 x 的偏导状态转移矩阵换成雅可比
H 乘 x 减h 乘 x 减测量预测从矩阵乘法换成非线性函数
HH 雅可比,即 h 对 x 的偏导观测矩阵换成雅可比
F P F 转置加 QF 雅可比 P F 雅可比转置加 Q形式不变,F 换雅可比
增益公式同形式H 换雅可比
I 减 K H 乘 P 减I 减 K H 雅可比乘 P 减形式不变,H 换雅可比

只有三样东西变了:

  • F 换成 F 雅可比。
  • H 换成 H 雅可比。
  • H 乘 x 换成 h 乘 x。

其他所有公式、推导、物理意义,完全不变。


十一、总结

五步心法

  • 第一步,状态预测。
  • 第二步,协方差预测,核心,决定 K。
  • 第三步,算最优增益 K,让校正后方差最小。
  • 第四步,用 K 把预测和测量融合。
  • 第五步,更新协方差,作为下一轮起点。

关键理解

P 减决定 K,K 决定校正,校正又更新 P。

  • P 减小,K 小,更信预测。
  • P 减大,K 大,更信测量。

协方差是因,增益是果。

从 KF 到 EKF

EKF 等于 KF 加一阶泰勒展开,把 F、H 换成雅可比。

五步结构不变,变的只是怎么预测和怎么算雅可比。

协方差递推仍是核心,EKF 只是把线性传播换成一阶线性化传播。

学习路线

KF 五步,协方差递推,最优增益推导,EKF 雅可比,UKF 或粒子滤波。


评论
成就一亿技术人!
拼手气红包6.0元
还能输入1000个字符
 
 条评论被折叠 查看
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付:10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包

打赏作者

极客 - L U

你的鼓励将是我创作的最大动力

¥1 ¥2 ¥4 ¥6 ¥10 ¥20
扫码支付:¥1
获取中
扫码支付

您的余额不足,请更换扫码支付或充值

打赏作者

实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值