卡尔曼滤波公式推导思想

卡尔曼滤波是一种递归最小二乘估计算法，广泛应用于信号处理、控制系统等领域，尤其适合于处理随机过程和动态系统的状态估计问题。其核心思想基于贝叶斯理论，通过预测和更新两个步骤来进行。

1. **预测步**（Predict step）：
-1}` 和系统模型 `F` 进行线性映射得到：`x_t = F x_{t-1} + w_t`，其中 `w_t` 是加权白噪声。
- 预测的协方差矩阵 `P_t^-` 也根据系统动态更新：`P_t^- = F P_{t-1} F^T + Q_t`，`Q_t` 是过程噪声的协方差矩阵。

2. **更新步**（Update step）：
- 接收到观测数据 `z_t` 后，计算残差 `y_t = z_t - H x_t^-`，其中 `H` 是从系统模型到测量的转换矩阵。
- 使用残差和观测数据的协方差 `R_t` 更新估计的状态后验概率分布：`K_t = P_t^- H^T (H P_t^- H^T + R_t)^{-1}`。
- 最终的估计状态 `x_t` 为：`x_t = x_t^- + K_t y_t`，新的协方差 `P_t` 为：`P_t = (I - K_t H) P_t^-`。

卡尔曼滤波的关键在于设计合适的转移矩阵 `F`、观测矩阵 `H`、过程噪声 `Q_t` 和测量噪声 `R_t`。这是一个迭代的过程，每一步都依赖于前一步的结果。