扩展卡尔曼滤波原理的公式

扩展卡尔曼滤波（EKF）是一种非线性系统状态估计方法，它在卡尔曼滤波的基础上，通过泰勒级数对非线性函数进行线性化，从而实现对非线性系统的状态估计。

EKF 的状态更新方程和卡尔曼滤波非常相似，只是在状态预测和状态更新的过程中，将线性化后的非线性函数代替了卡尔曼滤波中的线性矩阵。假设当前时刻为 $k$，状态向量为 $x_k$，观测向量为 $z_k$，非线性函数为 $f(\cdot)$，测量函数为 $h(\cdot)$，则 EKF 的状态更新方程可以表示为：

状态预测：
$$\hat{x}_{k|k-1} = f(\hat{x}_{k-1|k-1},u_{k-1})$$
$$P_{k|k-1} = F_{k-1}P_{k-1|k-1}F_{k-1}^T+Q_{k-1}$$

状态更新：
$$K_k = P_{k|k-1}H_k^T(H_kP_{k|k-1}H_k^T+R_k)^{-1}$$
$$\hat{x}_{k|k} = \hat{x}_{k|k-1}+K_k(z_k-h(\hat{x}_{k|k-1}))$$
$$P_{k|k} = (I-K_kH_k)P_{k|k-1}(I-K_kH_k)^T+K_kR_kK_k^T$$

其中，$F_k$ 是非线性函数 $f(\cdot)$ 在 $\hat{x}_{k-1|k-1}$ 处的一阶泰勒展开矩阵，$H_k$ 是测量函数 $h(\cdot)$ 在 $\hat{x}_{k|k-1}$ 处的一阶泰勒展开矩阵，$Q_k$ 是过程噪声协方差矩阵，$R_k$ 是观测噪声协方差矩阵，$u_{k-1}$ 是控制输入向量。