文章目录

Overview
Linear-Gaussian System
MAP →\rightarrow→ MLE →\rightarrow→ OLS
OLS →\rightarrow→ Gauss-Newton
Gauss-Newton →\rightarrow→ EKF(update P)
Gauss-Newton →\rightarrow→ IEKF →\rightarrow→ EKF(update X)
IEKF
Reference

Overview

持续更新：https://cgabc.xyz/posts/784a80cb/

本文主要包括以下几个方面：

将线性高斯系统的状态空间方程(运动和观测方程，即多个似然因子)组合为一个新的观测方程，其实就是将多个约束项放在一起
推导了MAP到MLE，再到OLS的历程
以上面系统的状态估计为出发点，推导了最小二乘、高斯牛顿、IEKF和EKF的区别与联系，将优化与滤波联系在一起，导出了高斯牛顿海塞矩阵H与EKF协方差矩阵P的关系，证明了IEKF与高斯牛顿的等价性，以及EKF即是高斯牛顿的一次迭代

Linear-Gaussian System

定义非线性系统

{xk−=g(xk−1)+v,v∼N(0,Q)yk=h(xk)+w,w∼N(0,R)\begin{cases} x_{k}^- = g(x_{k-1}) + v, \quad v \sim \mathcal{N}(0, Q) \\ y_{k} = h(x_k) + w, \quad w \sim \mathcal{N}(0, R) \end{cases} {xk−=g(xk−1)+v,v∼N(0,Q)yk=h(xk)+w,w∼N(0,R)

在工作点附近线性化 (在此不考虑状态预测，将状态先验定义为预测后的值)

{xk−=xk+v,v∼N(0,Q),xk−∼N(xk,Pk−)yk=h(xop,k)+H(xk−xop,k)+w,w∼N(0,Rk)\begin{cases} x_{k}^- = x_{k} + v, \quad v \sim \mathcal{N}(0, Q), \quad x_{k}^- \sim \mathcal{N}(x_k, P_k^-) \\ y_{k} = h(x_{op, k}) + H (x_k - x_{op, k}) + w, \quad w \sim \mathcal{N}(0, R_k) \end{cases} {xk−=xk+v,v∼N(0,Q),xk−∼N(xk,Pk−)yk=h(xop,k)+H(xk−xop,k)+w,w∼N(0,Rk)

其中，

Hk=∂h(xk)∂xk∣xop,kH_k = \frac{\partial h(x_k)}{\partial x_k} |_{x_{op,k}} Hk=∂xk∂h(xk)∣xop,k

合并所有 likelihood factors，即状态方程(先验) 与观测方程(后验) ，得到新的测量值、观测方程和残差

zk=[xk−yk],fk(xk)=[xkh(xk)],rk(xk)=zk−f(xk)z_k = \begin{bmatrix} x_{k}^- \\ y_k \end{bmatrix}, \quad f_k(x_k) = \begin{bmatrix} x_k \\ h(x_k) \end{bmatrix}, \quad r_k(x_k) = z_k - f(x_k) zk=[xk−yk],fk(xk)=[xkh(xk)],rk(xk)=zk−f(xk)

其中，

zk∼N(f(xk),Σk),Σk=[Pk−00Rk]z_k \sim \mathcal{N}(f(x_k), \Sigma_k), \quad \Sigma_k = \begin{bmatrix} P_k^- & 0 \\ 0 & R_k \end{bmatrix} zk∼N(f(xk),Σk),Σk=[Pk−00Rk]

新的观测方程 (工作点附近线性化，一阶泰勒展开)

f(xk)=f(xop,k)+Jk(xk−xop,k)+n≈f(xop,k)+JkΔxop,k+n\begin{aligned} f(x_k) =& f(x_{op,k}) + J_k (x_k - x_{op,k}) + n \\ \approx& f(x_{op,k}) + J_k \Delta x_{op,k} + n \end{aligned} f(xk)=≈f(xop,k)+Jk(xk−xop,k)+nf(xop,k)+JkΔxop,k+n

其中，

Jk=∂fk(x)∂xk∣xop,k=[IHk]\begin{aligned} J_k =& \frac{\partial f_k(x)}{\partial x_k} |_{x_{op,k}} = \begin{bmatrix} I \\ H_k \end{bmatrix} \end{aligned} Jk=∂xk∂fk(x)∣xop,k=[IHk]

测量残差

r(xk)=zk−f(xk)r(x_k) = z_k - f(x_k) r(xk)=zk−f(xk)

另外， 预测残差函数 (工作点附近线性化)

rk(xop,k)=zk−f(xop,k)=JkΔxop,k+n,n∼N(0,Σ)r_k(x_{op,k}) = z_k - f(x_{op,k}) = J_k \Delta x_{op,k} + n, \quad n \sim \mathcal{N}(0, \Sigma) rk(xop,k)=zk−f(xop,k)=JkΔxop,k+n,n∼N(0,Σ)

MAP →\rightarrow→ MLE →\rightarrow→ OLS

根据贝叶斯法则， 后验 = 似然 x 先验

P(X∣Z)⏟posterior=P(X,Z)P(Z)=P(Z∣X)P(X)P(Z)∝P(Z∣X)⏟likehoodP(X)⏟prior\begin{aligned} \underbrace{P(X \mid Z)}_{posterior} =& \frac{P(X, Z)}{P(Z)} \\ =& \frac{P(Z \mid X) P(X)}{P(Z)} \\ \propto& \underbrace{P(Z \mid X)}_{likehood} \underbrace{P(X)}_{prior} \end{aligned} posteriorP(X∣Z)==∝P(Z)P(X,Z)P(Z)P(Z∣X)P(X)likehoodP(Z∣X)priorP(X)

解决上述系统的状态估计问题，就是求X的最优估计使得 最大后验概率(MAP)，即求 最大似然估计(MLE)

X∗=arg⁡max⁡XP(X∣Z)=arg⁡max⁡XP(Z∣X)P(X)=arg⁡max⁡XP(Z∣X)\begin{aligned} X^* =& \arg \max_X P(X \mid Z) \\ =& \arg \max_X P(Z \mid X) P(X) \\ =& \arg \max_X P(Z \mid X) \\ \end{aligned} X∗===argXmaxP(X∣Z)argXmaxP(Z∣X)P(X)argXmaxP(Z∣X)

根据 多元高斯分布 的概率密度函数为

p(x∣μ,Σ)=N(x;μ,Σ)=1(2π)Ndet⁡Σexp⁡(−12(x−μ)TΣ−1(x−μ))p(\mathbf{x} \mid \boldsymbol{\mu}, \mathbf{\Sigma}) = \mathcal{N}(\mathbf{x} ; \boldsymbol{\mu}, \mathbf{\Sigma}) = \frac{1}{\sqrt{(2 \pi)^{N} \operatorname{det} \mathbf{\Sigma}}} \exp \left(-\frac{1}{2}(\mathbf{x}-\boldsymbol{\mu})^{T} \boldsymbol{\Sigma}^{-1}(\mathbf{x}-\boldsymbol{\mu})\right) p(x∣μ,Σ)=N(x;μ,Σ)=(2π)NdetΣ1exp(−21(x−μ)TΣ−1(x−μ))

对于上述系统 (Linear-Gaussian)，已知测量模型

z=f(x)+n,n∼N(0,Σ),z∼N(f(x),Σ)z = f(x) + n, \quad n \sim \mathcal{N}(0, \Sigma), \quad z \sim \mathcal{N}(f(x), \Sigma) z=f(x)+n,n∼N(0,Σ),z∼N(f(x),Σ)

则其似然概率

P(z∣x)=N(z;f(x),Σ)=ηexp⁡(−12(z−f(x))TΣ−1(z−f(x)))P(z \mid x) = \mathcal{N}(z; f(x), \Sigma) = \eta \exp \left(-\frac{1}{2}(z-f(x))^{T} {\Sigma}^{-1}(z-f(x))\right) P(z∣x)=N(z;f(x),Σ)=ηexp(−21(z−f(x))TΣ−1(z−f(x)))

定义马氏范数 (本文皆以此定义)

∥r∥Σ2≜rTΣ−1r\| r \|_{\Sigma}^2 \triangleq r^T \Sigma^{-1} r ∥r∥Σ2≜rTΣ−1r

因此，在 MLE 和高斯分布 的假设下，我们可以得到 最小二乘问题(OLS)

X∗=arg⁡min⁡X−log⁡(P(z∣x))=arg⁡min⁡X12(z−f(x))TΣ−1(z−f(x))=arg⁡min⁡X12∥z−f(x)∥Σ2=arg⁡min⁡X12∥z−f(xop)−JΔop∥Σ2=arg⁡min⁡X12∥JΔop−(z−f(xop))∥Σ2=arg⁡min⁡X12∥r(x)∥Σ2\begin{aligned} X^* =& \arg \min_X - \log \left( P(z \mid x) \right) \\ =& \arg \min_X \frac{1}{2} (z-f(x))^{T} {\Sigma}^{-1} (z-f(x)) \\ =& \arg \min_X \frac{1}{2} \| z-f(x) \|^2_{\Sigma} \\ =& \arg \min_X \frac{1}{2} \| z - f(x_{op}) - J \Delta_{op} \|^2_{\Sigma} \\ =& \arg \min_X \frac{1}{2} \| J \Delta_{op} - (z - f(x_{op})) \|^2_{\Sigma} \\ =& \arg \min_X \frac{1}{2} \| r(x) \|^2_{\Sigma} \end{aligned} X∗======argXmin−log(P(z∣x))argXmin21(z−f(x))TΣ−1(z−f(x))argXmin21∥z−f(x)∥Σ2argXmin21∥z−f(xop)−JΔop∥Σ2argXmin21∥JΔop−(z−f(xop))∥Σ2argXmin21∥r(x)∥Σ2

OLS →\rightarrow→ Gauss-Newton

根据以上最小二乘问题，我们可以得到G-N正规方程为

(JTΣ−1J)Δop=JTΣ−1rop(J^T \Sigma^{-1} J) \Delta_{op} = J^T \Sigma^{-1} r_{op} (JTΣ−1J)Δop=JTΣ−1rop

从而，状态增量

Δop=(JTΣ−1J)−1JTΣ−1rop\Delta_{op} = (J^T \Sigma^{-1} J)^{-1} J^T \Sigma^{-1} r_{op} Δop=(JTΣ−1J)−1JTΣ−1rop

另外，对于 加权最小二乘，上式中的 Σ−1\Sigma^{-1}Σ−1 (信息矩阵，协方差的逆)，就是 权重矩阵 WWW；方差越大，权重越小。

Gauss-Newton →\rightarrow→ EKF(update P)

后验协方差矩阵

Pk+=E[(xk+−xop,k)(xk+−xop,k)T]=E(ΔopΔopT)=E((JTΣ−1J)−1JTΣ−1ropropTΣ−TJ(JTΣ−1J)−T)=(JTΣ−1J)−1JTΣ−1E(ropropT)Σ−1JJ−1ΣJ−T=(JkTΣk−1Jk)−1=([IHkT][Pk−00Rk]−1[IHk])−1=(P−1+HTR−1H)−1=P−PHT(HPHT+R)−1HP(Matrix inversion lemmas)=(I−KH)Pk−\begin{aligned} P_k^+ =& E[(x_k^+ - x_{op,k})(x_k^+ - x_{op,k})^T] \\ =& E(\Delta_{op} \Delta_{op}^T) \\ =& E( (J^T \Sigma^{-1} J)^{-1} J^T \Sigma^{-1} r_{op} r_{op}^T \Sigma^{-T} J (J^T \Sigma^{-1} J)^{-T}) \\ =& (J^T \Sigma^{-1} J)^{-1} J^T \Sigma^{-1} E(r_{op} r_{op}^T) \Sigma^{-1} J J^{-1} \Sigma J^{-T} \\ =& \left( {\color{blue}{J_k^T \Sigma_k^{-1} J_k}} \right)^{-1} \\ =& \left( \begin{bmatrix} I & H_k^T \end{bmatrix} \begin{bmatrix} P_k^- & 0 \\ 0 & R_k \end{bmatrix}^{-1} \begin{bmatrix} I \\ H_k \end{bmatrix} \right)^{-1} \\ =& \left( P^{-1} + H^T R^{-1} H \right)^{-1} \\ =& P - P H^T (H P H^T + R)^{-1} H P \quad \quad \text{(Matrix inversion lemmas)} \\ =& {\color{blue}{(I - KH) P_k^-}} \end{aligned} Pk+=========E[(xk+−xop,k)(xk+−xop,k)T]E(ΔopΔopT)E((JTΣ−1J)−1JTΣ−1ropropTΣ−TJ(JTΣ−1J)−T)(JTΣ−1J)−1JTΣ−1E(ropropT)Σ−1JJ−1ΣJ−T(JkTΣk−1Jk)−1([IHkT][Pk−00Rk]−1[IHk])−1(P−1+HTR−1H)−1P−PHT(HPHT+R)−1HP(Matrix inversion lemmas)(I−KH)Pk−

其中，K即卡尔曼增益

K=Pk−HT(HPk−HT+Rk)−1=(HTR−1H+P−1)−1HTR−1(Matrix inversion lemmas)\begin{aligned} K =& P_k^- H^T(H P_k^- H^T + R_k)^{-1} \\ =& (H^T R^{-1}H + P^{-1})^{-1} H^T R^{-1} \quad \quad \text{(Matrix inversion lemmas)} \end{aligned} K==Pk−HT(HPk−HT+Rk)−1(HTR−1H+P−1)−1HTR−1(Matrix inversion lemmas)

因此，高斯牛顿的海塞(信息)矩阵H 等价于 EKF的协方差矩阵P

H−1=(JkTΣk−1Jk)−1⟷P{\color{red}{ H^{-1} = \left(J_k^T \Sigma_k^{-1} J_k\right)^{-1} \longleftrightarrow P }} H−1=(JkTΣk−1Jk)−1⟷P

Gauss-Newton →\rightarrow→ IEKF →\rightarrow→ EKF(update X)

根据先验，我们从 高斯-牛顿 出发，推导 IEKF的后验估计状态更新方式 (以下 xi=xop,ix_i = x_{op,i}xi=xop,i)

xi+1=xi+Δx=xi+(JTΣ−1J)−1(JTΣ−1rop)=(JTΣ−1J)−1JTΣ−1(z−f(xi)+Jxi)=(P−1+HTR−1H)−1[P−1HTR−1][xi−yi−h(xi)+Hxi]=(P−1+HTR−1H)−1(HTR−1(yi−h(xi)+Hxi)+P−1xi−)=(P−1+HTR−1H)−1(HTR−1(yi−h(xi)−H(xi−−xi)+Hxi−)+P−1xi−)=xi−+(P−1+HTR−1H)−1HTR−1(yi−h(xi)−H(xi−−xi))=xi−+K(yi−h(xi)−H(xi−−xi))\begin{aligned} x_{i+1} =& x_i + \Delta x \\ =& x_i + (J^T \Sigma^{-1} J)^{-1} (J^T \Sigma^{-1} r_{op}) \\ =& (J^T \Sigma^{-1} J)^{-1} J^T \Sigma^{-1} (z-f(x_i) + J x_i) \\ =& \left( P^{-1} + H^T R^{-1} H \right)^{-1} \begin{bmatrix} {P}^{-1} & H^T R^{-1} \end{bmatrix} \begin{bmatrix} x_i^- \\ y_i - h(x_i) + H x_i \end{bmatrix} \\ =& \left( P^{-1} + H^T R^{-1} H \right)^{-1} \left( H^T R^{-1}(y_i - h(x_i) + H x_i) + {P}^{-1} x_i^- \right) \\ =& \left( P^{-1} + H^T R^{-1} H \right)^{-1} \left( H^T R^{-1}(y_i - h(x_i) - H(x_i^- - x_i) + H x_i^-) + {P}^{-1} x_i^- \right) \\ =& x_i^- + {\color{blue}{ \left( P^{-1} + H^T R^{-1} H \right)^{-1} H^T R^{-1} }} (y_i - h(x_i) - H(x_i^- - x_i)) \\ =& {\color{blue}{ x_i^- + K (y_i - h(x_i) - H(x_i^- - x_i)) }} \end{aligned} xi+1========xi+Δxxi+(JTΣ−1J)−1(JTΣ−1rop)(JTΣ−1J)−1JTΣ−1(z−f(xi)+Jxi)(P−1+HTR−1H)−1[P−1HTR−1][xi−yi−h(xi)+Hxi](P−1+HTR−1H)−1(HTR−1(yi−h(xi)+Hxi)+P−1xi−)(P−1+HTR−1H)−1(HTR−1(yi−h(xi)−H(xi−−xi)+Hxi−)+P−1xi−)xi−+(P−1+HTR−1H)−1HTR−1(yi−h(xi)−H(xi−−xi))xi−+K(yi−h(xi)−H(xi−−xi))

从而验证了他们在数学上的等价性

IEKF⟷Gauss-Newon{\color{red}{ \text{IEKF} \longleftrightarrow \text{Gauss-Newon} }} IEKF⟷Gauss-Newon

当第一次迭代时，一般 xi=xi−x_i = x_i^-xi=xi−，此时我们可以得到 EKF的状态更新公式

xi+1=xi−+K(yi−h(xi))x_{i+1} = x_i^- + K (y_i - h(x_i) ) xi+1=xi−+K(yi−h(xi))

因此，EKF 等价于高斯牛顿的一次迭代

EKF⟷IEKF一次迭代⟷GN一次迭代{\color{red}{\text{EKF} \longleftrightarrow \text{IEKF一次迭代} \longleftrightarrow \text{GN一次迭代}}} EKF⟷IEKF一次迭代⟷GN一次迭代

IEKF

Reference

The Iterated Kalman Filter Update as a Gauss-Newton Method
Performance evaluation of iterated extended Kalman filter with variable step-length
IKF(IEKF)推导
IEKF 和 Gaussian-Newton method 等价性证明
Matrix inversion lemmas

From MAP, MLE, OLS, G-N to IEKF,EKF相关推荐

机器学习-白板推导系列(一)-绪论（机器学习的MLE(最大似然估计)和MAP(最大后验估计)）
频率学派 - Frequentist - Maximum Likelihood Estimation (MLE,最大似然估计) 贝叶斯学派 - Bayesian - Maximum A Posteri ...
hdoj 5199 Gunner map
Gunner Time Limit: 1 Sec Memory Limit: 256 MB 题目连接 http://acm.hdu.edu.cn/showproblem.php?pid=5199 D ...
【Leafletjs】4.L.Map 中文API
L.Map API各种类中的核心部分,用来在页面中创建地图并操纵地图. 使用 example // initialize the map on the "map" div with ...
Set集合[HashSet,TreeSet,LinkedHashSet]，Map集合[HashMap,HashTable,TreeMap]
------------ Set ------------------- 有序: 根据添加元素顺序判定, 如果输出的结果和添加元素顺序是一样无序: 根据添加元素顺序判定,如果输出的结果和添加元素的顺 ...
【JS】446- 你不知道的 map
本文来自[前端早读课],内容不错,推荐给大家. 前言今日早读文章由酷家乐@Gloria投稿分享. 正文从这开始-- 作为前端工程师,你肯定用过Array.prototype.map方法. 如果你听说 ...
【论文阅读】GETNext: Trajectory Flow Map Enhanced Transformer for Next POI Recommendation
[论文阅读]GETNext: Trajectory Flow Map Enhanced Transformer for Next POI Recommendation 前言 Next POI 推荐是根 ...
天梯赛 L2-030 冰岛人 (25 分) map
L2-030 冰岛人 (25 分) 2018年世界杯,冰岛队因1:1平了强大的阿根廷队而一战成名.好事者发现冰岛人的名字后面似乎都有个"松"(son),于是有网友科普如下: 冰岛人 ...
Emmet 文档下载，所有快捷键总结
为什么80%的码农都做不了架构师?>>> Emmet | Cheat Sheet (2017-03) Syntax . . . . . . . . 1 Child: > ...
文献阅读 - Combining Sketch and Tone for Pencil Drawing Production
Combining Sketch and Tone for Pencil Drawing Production C. W. Lu, L. Xu, J. Y. Jia, Combining Sketch ...

From MAP, MLE, OLS, G-N to IEKF,EKF