卡尔曼滤波从零到精通

文章目录

- 一、为什么需要卡尔曼滤波
- 二、温度例子
- 三、符号系统全解
- 四、卡尔曼滤波五步推导
- [五、Q 和 R 到底是什么](#五、Q 和 R 到底是什么)
- 六、五步闭环总览
- [七、从 KF 升级到 EKF](#七、从 KF 升级到 EKF)
- [八、雅可比矩阵：EKF 的灵魂](#八、雅可比矩阵：EKF 的灵魂)
- [九、EKF 与 KF 对照](#九、EKF 与 KF 对照)
- 十、哪些东西被非线性函数替代
- 十一、总结

【全文大纲】 : 主页目录

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

系统面对两个天然敌人：

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

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

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

这个权重就是卡尔曼增益 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} \[TkVk\]=\[10Δt1\]\[Tk−1Vk−1

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

观测矩阵 H

H = $1 0$ H=\begin{bmatrix}1&0\end{bmatrix} H= $10$

为什么存在：状态是二维，测量是一维，需要一个矩阵把状态投影到测量空间。
为什么是 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= $10$ $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= $21aΔ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= $P00P10P01P11$

元素	含义
P00	温度误差的方差
P11	速率误差的方差
P01 等于 P10	温度与速率误差的协方差

P 减：上标负号表示预测阶段，还没用测量校正。
P 无负号：校正阶段，用了测量之后。

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

卡尔曼增益 K

K = $K 0 K 1$ K=\begin{bmatrix}K_0\\K_1\end{bmatrix} K= $K0K1$

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^

代码：

c 复制代码

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

代码：

c 复制代码

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

代码：

c 复制代码

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)$ = $K0K1$

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^−)

代码：

c 复制代码

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^−= $10$ $TpredVpred$ =Tpred

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

二维时 K 如何分配：

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

输入：x 减，K，z。

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

第五步：协方差更新

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

代码：

c 复制代码

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= $1001$

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= $K0K100$

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-K101$

逐元素乘：

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

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

P 10 = P 10 − − K 1 P 00 − P_{10}=P_{10}^--K_1P_{00}^- P10=P10−−K1P00−

P 11 = P 11 − − K 1 P 01 − P_{11}=P_{11}^--K_1P_{01}^- P11=P11−−K1P01−

为什么 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 减。

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

对称化：

c 复制代码

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+21aΔ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= $21aΔ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 = $\partial f 1 \partial x 1 \dots \partial f 1 \partial x n ⋮ ⋱ ⋮ \partial f m \partial x 1 \dots \partial f m \partial 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 = $\partial f 1 \partial x 1 \partial f 1 \partial x 2 \partial f 2 \partial x 1 \partial f 2 \partial 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= $\partialx1\partialf1\partialx1\partialf2\partialx2\partialf1\partialx2\partialf2$

具体例子

非线性模型：

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+ax22x2$

雅可比：

F = $1 2 a x 2 0 1$ F=\begin{bmatrix} 1&2ax_2\\ 0&1 \end{bmatrix} F= $102ax21$

注意：雅可比里有 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 对照

步骤	KF	EKF
状态预测	x 减等于 F 乘 x	x 减等于 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 乘 x	f 乘 x 和 u	状态转移从矩阵乘法换成非线性函数
F	F 雅可比，即 f 对 x 的偏导	状态转移矩阵换成雅可比
H 乘 x 减	h 乘 x 减	测量预测从矩阵乘法换成非线性函数
H	H 雅可比，即 h 对 x 的偏导	观测矩阵换成雅可比
F P F 转置加 Q	F 雅可比 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 或粒子滤波。