EKF的误差传播

非线性函数的线性化近似

  对于一个状态量$X\in {\mathbb{R}}^n$，t时刻的状态表示为$X_t$，t+1时刻的状态表示为$X_{t+1}$。前后两个时刻的状态可以用一个非线性的运动学函数表示$f(\cdot )$。
$$X_{t+1}=f(X_t+n_t)$$
其中$n_t\sim \mathcal{N}(0,R)$是噪声项。注意，通常情况下，函数包含了旋转和平移等操作，因此是非线性的。在非线性的情况下，如果我们知道了$X_t$的概率分布，想要推导$X_{t+1}$的概率分布往往是很困难的，因此我们需要对该函数进行一些必要的近似使推导$X_{t+1}$分布的过程变得简单一些。这个近似就是取$f(\cdot )$的一阶近似：
$$X_{t+1}\approx f(X_{opt})+F_t\delta X_t+G_t{n}_t$$
$$F_t=\frac{\partial f(X_{opt}+n_t)}{\partial X_{opt}}$$
$$G_t=\frac{\partial f(X_{opt}+n_t)}{\partial n_t}$$
其中线性展开的点我们成为工作点，这个工作点的选择影响到了线性化近似的好坏，即展开点附近如果函数的形状接近于线性的话，则近似的效果较好，如果很弯曲的话则近似的效果则很差。以上就是非线性函数进行线性化近似的基本思路。

误差项的传播

实际情况下，我们的估计中总是带有一定的误差，即上式的中的$X=\hat{X}+\delta X$，$\hat{X}$为当前估计量。因此上式我们可以写为：
$${\check{X}}_{t+1}=f({\hat{X}}_t+\delta X_t+n_t)$$
${\check{X}}_{t+1}$表示一步状态传递得到的$X_{t+1}$的状态预测（之后还会用测量值进行修正得到最终的${\hat{X}}_{t+1}$）
EKF往往选择将$\hat{X}$做为上述提到的线性化近似的展开点。(当然你也可以选择别的点作为展开点，如IEKF就通过多次迭代，每一次的展开点都不一样)
$${\check{X}}_{t+1}\approx f({\hat{X}}_t)+F_t\delta X_t+G_t{n}_t$$
$${\check{X}}_{t+1}-f({\hat{X}}_t)=F_t\delta X_t+G_t{n}_t$$
$$\delta X_{t+1}=F_t\delta X_t+G_t{n}_t$$
$${\Sigma }_{t+1}=E[\delta X_{t+1}\delta X_{t+1}^T]=E[(F_t\delta X_t+G_t{n}_t)(F_t\delta X_t+G_t{n}_t{)}^T]=F_t{\Sigma }_t{F}_t^T+G_{t}R{G}_t^T$$
${\Sigma }_{t+1}$为t+1时刻的协方差矩阵，${\Sigma }_{t+1}$为t时刻的协方差矩阵。