扩展卡尔曼滤波

扩展卡尔曼滤波（extended Kalman filter，EKF）通过在当前估计附近线性化非线性模型，递推近似状态均值与协方差。它把模型预测与观测残差结合，但线性化后的高斯近似一般不等于真实后验，初始误差较大或观测明显非线性时可能产生显著偏差。

预测与校正的时序

考虑加性噪声模型 $x_k=f(x_{k-1},u_{k-1})+w_k$、$z_k=h(x_k)+v_k$，其中状态维数 $n$，观测维数 $p$。过程噪声与观测噪声为零均值高斯噪声、协方差分别为 $Q_k,R_k$，并与此前状态和彼此独立。以 $m_{k-1},P_{k-1}$ 表示上次近似均值和协方差，先计算 $$m_k^{-}=f(m_{k-1},u_{k-1}),F_k=\frac{\partial f}{\partial x}(m_{k-1},u_{k-1}),P_k^{-}=F_k{P}_{k-1}{F}_k^{𝖳}+Q_k.$$ 再在预测均值处计算 $H_k=\partial h/\partial x(m_k^{-})$，得到创新 $r_k=z_k-h(m_k^{-})$、协方差 $S_k=H_k{P}_k^{-}{H}_k^{𝖳}+R_k$。假定 $S_k$ 可逆，更新为 $$K_k=P_k^{-}{H}_k^{𝖳}{S}_k^{-1},m_k=m_k^{-}+K_k{r}_k,P_k=P_k^{-}-K_k{S}_k{K}_k^{𝖳}.$$ 这些是在线性化近似中成立的条件高斯更新，而不是任意非线性模型的精确条件分布。

平方观测的一次更新

构造标量预测 $X\approx N(2,1/4)$，观测为 $z=X^2+v$，$v\sim N(0,1)$，本次读数为五。线性化 $h(x)=x^2$ 在二处给出 $h(2)=4,H=4$，所以 $S=16\times 1/4+1=5$、$K=(1/4)4/5=1/5$。创新为一，更新均值 $m=11/5=2.2$，方差 $P=1/4-(1/5{)}^2\times 5=1/20$。

EKF用切线近似当前附近的观测关系；更新得到的x均值不是简单解方程x平方等于观测值。

之所以不直接取 $\sqrt{5}$，是因为观测含噪且先验也提供信息；平方方程还有负根，一次观测本来就不能普遍区分正负。EKF选择的是围绕当前估计的一个局部高斯表示。

线性化失败的具体方向

若预测均值为零，平方观测的导数也为零，因此EKF给出 $K=0$，不论这次读数是什么都不更新均值，并在这个线性化公式下不从观测减少方差。真实平方观测依然包含幅值信息，后验可能出现正负两峰。这不是“观测没有信息”，而是当前切线忽略了它。无迹卡尔曼滤波改变了矩传播近似，但也不保证能完整表示多峰后验。

标量更新的Python复核

以下只实现本例平方观测的一次校正，输入为预测均值、非负方差、读数和正测量方差。采用Joseph形式 $P=(1-KH{)}^2{P}^{-}+K^{2}R$ 计算新方差，减少浮点相消风险；时间和额外空间均为常数，不改写输入。

Python 3

import math

def ekf_square_update(mean, variance, observation, noise_variance):
    data = (mean, variance, observation, noise_variance)
    if not all(math.isfinite(x) for x in data):
        raise ValueError("finite data required")
    if variance < 0 or noise_variance <= 0:
        raise ValueError("nonnegative prior variance and positive noise required")
    H = 2.0 * mean
    S = H * H * variance + noise_variance
    K = variance * H / S
    updated_mean = mean + K * (observation - mean * mean)
    updated_variance = (1.0 - K * H) ** 2 * variance + K * K * noise_variance
    return updated_mean, updated_variance

print(ekf_square_update(2, 0.25, 5, 1))
print(ekf_square_update(0, 1, 5, 1))

输出约为 $(2.2,0.05)$ 和 $(0,1)$。变式。 第一例将读数改为四，创新为零，均值仍为二，但方差仍降为 $1/20$；获得一个与预测一致的读数，也能改变不确定性。

实际滤波还需检查模型、噪声尺度、雅可比、角度残差等坐标约定，以及数值协方差的对称性和半正定性。EKF中的 $P$ 是算法的近似不确定性描述，不能仅因其数值很小就宣称真实误差也很小。