扩展卡尔曼滤波公式推导

卡尔曼滤波是一种用于估计系统状态的递归滤波器。它通过结合系统动态模型和观测数据来提供对状态的最优估计。下面是卡尔曼滤波的公式推导过程：

1. 系统模型：
状态方程：x_k = F_k * x_{k-1} + B_k * u_k + w_k
观测方程：z_k = H_k * x_k + v_k

其中，x_k表示系统在时间步k时的状态向量，u_k表示控制输入，z_k表示观测向量。

F_k是状态转移矩阵，描述系统状态从时间步k-1到时间步k的演化关系。
B_k是控制输入矩阵，描述控制输入对系统状态的影响。
w_k是系统过程噪声，假设其均值为0，协方差矩阵为Q_k。

H_k是观测矩阵，描述观测向量与系统状态之间的关系。
v_k是观测噪声，假设其均值为0，协方差矩阵为R_k。

2. 预测步骤（时间更新）：
根据上一时刻的状态估计x_{k-1}和控制输入u_{k-1}，通过状态转移矩阵F_k和控制输入矩阵B_k进行状态预测：
x_{k}^- = F_k * x_{k-1} + B_k * u_{k-1}

同时，计算预测误差协方差矩阵P_{k}^-：
P_{k}^- = F_k * P_{k-1} * F_k^T + Q_k

3. 更新步骤（测量更新）：
根据当前观测向量z_k，通过观测矩阵H_k对预测状态进行观测估计：
y_k = z_k - H_k * x_{k}^-

计算观测残差协方差矩阵S_k：
S_k = H_k * P_{k}^- * H_k^T + R_k

计算卡尔曼增益K_k：
K_k = P_{k}^- * H_k^T * S_k^{-1}

更新状态估计值：
x_{k} = x_{k}^- + K_k * y_k

更新误差协方差矩阵：
P_{k} = (I - K_k * H_k) * P_{k}^-

其中，^-表示预测值，而没有^-表示更新值。

以上就是卡尔曼滤波的公式推导过程。通过递归地进行预测和更新步骤，卡尔曼滤波能够对系统状态进行最优估计。请注意，以上推导过程是基于线性系统和高斯噪声的情况，对于非线性系统还需要进行扩展，例如使用扩展卡尔曼滤波（Extended Kalman Filter）或无迹卡尔曼滤波（Unscented Kalman Filter）等方法。