迭代扩展卡尔曼滤波的公式推导就行

迭代扩展卡尔曼滤波（Iterated Extended Kalman Filtering，IEKF）是一种改进的迭代卡尔曼滤波算法，用于非线性系统的状态估计。它通过迭代优化步骤来逼近非线性函数，提高状态估计的准确性。下面是IEKF的公式推导概述：

假设我们有一个非线性系统的状态方程：
x_k = f(x_{k-1}, u_{k-1}) + w_{k-1}

其中，x_k是系统在时刻k的状态，f是非线性函数，u_{k-1}是控制输入，w_{k-1}是过程噪声。

观测方程可以表示为：
z_k = h(x_k) + v_k

其中，z_k是观测值，h是非线性函数，v_k是观测噪声。

IEKF的主要思想是通过迭代优化来逼近非线性函数，并使用线性化的形式进行状态预测和更新。

1. 线性化：在每次迭代中，我们首先线性化非线性函数f和h，并计算其雅可比矩阵F和H。
F_k = ∂f/∂x|_{x=x_{k-1}} (状态转移矩阵)
H_k = ∂h/∂x|_{x=x_k} (观测矩阵)

2. 预测：
a. 预测状态估计：
x^-_k = f(x^+_{k-1}, u_{k-1})
b. 预测协方差矩阵：
P^-_k = F_k P^+_{k-1} F_k^T + Q_k
其中，Q_k是过程噪声的协方差矩阵。

3. 更新：
a. 计算增益矩阵：
K_k = P^-_k H_k^T (H_k P^-_k H_k^T + R_k)^-1
其中，R_k是观测噪声的协方差矩阵。
b. 计算观测残差：
y_k = z_k - h(x^-_k)
c. 更新状态估计：
x^+_k = x^-_k + K_k y_k
d. 更新协方差矩阵：
P^+_k = (I - K_k H_k) P^-_k
e. 判断是否收敛，如果未收敛，则回到预测步骤进行下一次迭代。

通过反复进行预测和更新步骤，迭代扩展卡尔曼滤波可以逐渐逼近非线性函数，提高状态估计的准确性。需要注意的是，IEKF的收敛性和性能与初始状态估计和协方差矩阵的选择有关。因此，在实际应用中，初始状态估计的准确性对IEKF的效果有一定影响。