请完整推导卡尔曼滤波方程和扩展卡尔曼滤波方程

卡尔曼滤波是一种用于估计系统状态的递归滤波器，它适用于线性动态系统和高斯噪声模型。其基本思想是通过组合系统的预测和测量信息来获得更准确的状态估计。

卡尔曼滤波方程推导如下：

1. 系统模型：
假设我们有一个线性动态系统，可以用以下方程表示：
x(k) = A * x(k-1) + B * u(k-1) + w(k-1)
其中，x(k)是系统在时间步k的状态向量，A是状态转移矩阵，B是输入矩阵，u(k-1)是系统在时间步(k-1)的输入向量，w(k-1)是过程噪声。

2. 测量模型：
系统的测量方程可以表示为：
z(k) = H * x(k) + v(k)
其中，z(k)是系统在时间步k的观测向量，H是观测矩阵，v(k)是观测噪声。

3. 预测步骤：
根据系统模型，我们可以进行状态预测：
x^(k|k-1) = A * x^(k-1|k-1) + B * u(k-1)
P(k|k-1) = A * P(k-1|k-1) * A^T + Q
其中，x^(k|k-1)是对系统状态的先验估计，P(k|k-1)是状态估计的协方差矩阵，Q是过程噪声的协方差矩阵。

4. 更新步骤：
根据测量模型，我们可以进行状态更新：
K(k) = P(k|k-1) * H^T * (H * P(k|k-1) * H^T + R)^-1
x^(k|k) = x^(k|k-1) + K(k) * (z(k) - H * x^(k|k-1))
P(k|k) = (I - K(k) * H) * P(k|k-1)
其中，K(k)是卡尔曼增益，x^(k|k)是对系统状态的后验估计，P(k|k)是状态估计的协方差矩阵，R是观测噪声的协方差矩阵，I是单位矩阵。

扩展卡尔曼滤波（EKF）是卡尔曼滤波的非线性扩展，主要针对非线性系统模型。它通过线性化非线性模型来近似地应用卡尔曼滤波算法。

扩展卡尔曼滤波方程推导同样分为预测步骤和更新步骤，只不过在预测步骤中需要对非线性系统模型进行线性化。

预测步骤：
1. 根据非线性系统模型，进行状态预测：
x^(k|k-1) = f(x^(k-1|k-1), u(k-1))
F(k) = ∂f/∂x(k-1)|x^(k-1|k-1), u(k-1)
P(k|k-1) = F(k) * P(k-1|k-1) * F(k)^T + Q
其中，f表示非线性状态转移函数，F(k)是状态转移函数f的雅可比矩阵。

更新步骤：
1. 根据非线性观测模型，进行状态更新：
z^(k) = h(x^(k|k-1))
H(k) = ∂h/∂x(k)|x^(k|k-1)
K(k) = P(k|k-1) * H(k)^T * (H(k) * P(k|k-1) * H(k)^T + R)^-1
x^(k|k) = x^(k|k-1) + K(k) * (z(k) - z^(k))
P(k|k) = (I - K(k) * H(k)) * P(k|k-1)
其中，h表示非线性观测函数，H(k)是观测函数h的雅可比矩阵。

以上就是卡尔曼滤波方程和扩展卡尔曼滤波方程的推导过程。