PS：EKF本身是一种数学工具，具体的原理推导可以参考文献[1]。对于使用者来说，只要理解这个工具对应的数学模型，以及对应系数的更新方式，就可以套用EKF的求解模式进行求解。
1 模型
在离散线性系统中，对于某种状态\(\mathcal{x}\)
定义运动方程
(1.1)
$$
\mathcal{x}_{k} = A_{k-1} \mathcal{x}_{k-1} + \mathcal{v}_k + \mathcal{w}_k
$$
公式中\(\mathcal{w}_k \in \mathcal{N}(\mathbf{0}, Q_k)\)的过程噪声，其中\(A_{k-1}\)称为转移矩阵，\(\mathcal{x}_0 \in \mathcal{N}(\mathbf{0}, P_0)\),\(P_0\)为\(\mathcal{x}_0\)的初始协方差矩阵。
同样，定义观测方程
(1.2)
$$
\mathcal{y}_{k} = C_k \mathcal{x}_k + \mathcal{n}_k
$$
公式中\(\mathcal{n}_k \in \mathcal{N}(\mathbf{0}, R_k)\)的测量噪声,其中\(C_{k}\)称为观测矩阵。
2 卡尔曼滤波
下面为常规的扩展卡尔曼滤波更新过程[1]
1.预测
(2.1)
$$
\begin{aligned}
P_k &= A_{k-1}P_{k-1}A_{k-1}^T + Q_k\\
\mathcal{x}_k &=A_{k-1}\mathcal{x}_{k-1}+\mathcal{v}_k
\end{aligned}
$$
2.卡尔曼增益
(2.2)
$$
\begin{aligned}
K &= P_kC_k^T(C_kP_kC_k^T+R_k)^{-1}\\
\end{aligned}
$$
3.更新
(2.3)
$$
\begin{aligned}
P_k&= (I – KC_k)P_k\\
\mathcal{x}_k &= \mathcal{x}_k + K(\mathcal{y}_k – C_k\mathcal{x}_k)
\end{aligned}
$$
即，每次可以使用公式(2.1),(2.2),(2.3)进行过程更新。
3 理解
在每一轮更新中，先根据运动方程进行预测，然后结合观测方程估计出一个修正系数，最后使用系数对变量进行修正。
仔细观察上面更新过程以及对应数学模型，可以发现，在本质上来说可以通过运动方程直接递推出下一个状态，但是考虑到一些噪音（为了提高精度），这里会使用观测方程来对递推出的状态进行修正，也因此可以理解成两类方程耦合
注意，上面公式中协方差矩阵\(P_{k}\)本应该服务的对象，下面将以SLAM当中的例子加以说明。
4 SLAM当中的EKF
4.1 运动方程
对于位姿\(T\)，以及它对应的广义速度\(\varpi\)，它们满足运动学方程
(4.1)
$$
\dot{T} = \varpi ^\wedge T
$$
对上面的两个变量，分边添加一个微小的扰动，然后忽略二阶小量有
(4.2)
$$
\begin{aligned}
\frac{d}{dt}((I + \delta \xi^\wedge)T) &= (\delta \varpi + \varpi)^\wedge ((I +\delta \xi ^\wedge) T)\\
\delta \dot{\xi}^\wedge T + \dot{T} + \delta \xi ^\wedge \dot{T} &=\varpi ^\wedge T + \varpi ^\wedge \delta \xi^\wedge T + \delta \varpi^\wedge T + \delta \varpi^\wedge \delta \xi ^\wedge T\\
& \approx \varpi ^\wedge T + \varpi ^\wedge \delta \xi^\wedge T + \delta \varpi^\wedge T
\end{aligned}
$$
将(4.1)带入(4.2)有
(4.3)
$$
\begin{aligned}
\delta \dot{\xi}^\wedge &= \varpi ^\wedge \delta \xi ^\wedge – \delta \xi ^\wedge \varpi ^\wedge + \delta \varpi ^\wedge\\
&=(\varpi ^\curlywedge \delta \xi )^\wedge+ \delta \varpi ^\wedge\\
&=(\varpi ^\curlywedge \delta \xi + \delta \varpi)^\wedge
\end{aligned}
\\
\Downarrow
\\
\delta \dot{\xi} = \varpi ^\curlywedge \delta \xi + \delta \varpi
$$
一般把(4.1)称为标称运动学方程，(4.2)称为扰动运动学方程。
对上面公式进行离散化可得到
(4.4)
$$
\begin{aligned}
T_k &= \exp((t_k – t_{k-1}) \varpi ^\wedge)T_{k-1}\\
\delta \xi_k &= \exp((t_k – t_{k-1})\varpi ^\curlywedge) \delta \xi _{k-1} + \mathcal{w}_k
\end{aligned}
$$
\(t_{k}\)表示对应时刻值，\(\mathcal{w}_{k} \in \mathcal{N}(0, Q_k)\)为过程噪声。
4.2 观测方程
这里假设具有如下的观测模型
(4.5)
$$
\mathcal{y} = T \mathcal{p} + n
$$
同样，两边同时加一个微小的扰动有
(4.6)
$$
\begin{aligned}
\mathcal{y} + \delta \mathcal{y} &= (I+ \delta \xi^\wedge)Tp + n\\
&= Tp + \delta \xi ^\wedge Tp + n\\
&= Tp + (Tp)^ {\odot} \delta \xi + n
\end{aligned}
$$
这里将(4.5)的标称解带入(4.6)有
(4.7)
$$
\delta y = (Tp)^{\odot} \delta \xi + n
$$
即有最终离散化公式
(4.8)
$$
\delta y_k = (Tp)^{\odot} \delta \xi_k + n_k
$$
上面\(\mathcal{n}_k \in \mathcal{N}(\mathbf{0}, R_k)\)
4.3 EKF过程
这里先约定如下变量
(4.9)
$$
\begin{aligned}
O_k &= \exp((t_k – t_{k-1}) \varpi_{k-1} ^\wedge) \\
G_k &= \exp((t_k – t_{k-1})\varpi_{k-1} ^\curlywedge)\\
C_k &= (T_kp_k)^{\odot}
\end{aligned}
$$
最终结果如下
4.3.1 预测
(4.10)
$$
\begin{aligned}
T_k &= O_k T_{k-1}\\
P_k &= G_k P_{k-1}G_k^T + Q_k
\end{aligned}
$$
4.3.2 卡尔曼增益
(4.11)
$$
\begin{aligned}
K = P_k C_k^T (C_k P_k C_k^T + R_k)^{-1}
\end{aligned}
$$
4.3.3 矫正
(4.12)
$$
\begin{aligned}
P_k &= (I – KC_k)P)k\\
\mathcal{y}_k' &= T_k \mathcal{p}_k\\
T_k &= \exp((K(\mathcal{y}_k – \mathcal{y}_k'))^\wedge)T_k
\end{aligned}
$$
参考
[1] 高翔,谢晓佳. 机器人学中的状态估计[M]. 西安交通大学出版社,2018