无迹卡尔曼滤波算法推导

无迹卡尔曼滤波（Unscented Kalman Filter，UKF）是一种非线性滤波算法，用于估计系统状态。它通过在状态空间中选择一组特定的采样点（称为sigma点），来近似非线性函数的传播和测量模型。以下是无迹卡尔曼滤波算法的推导过程：

1. 初始化：
- 初始化状态向量 x 和协方差矩阵 P。
- 初始化过程噪声协方差矩阵 Q 和测量噪声协方差矩阵 R。

2. 预测步骤：
- 通过选择 sigma 点来近似状态向量和协方差矩阵。
- 通过非线性传播函数 f 对 sigma 点进行传播，得到预测状态向量 x' 和预测协方差矩阵 P'。
- 计算预测测量值的均值和协方差矩阵。

3. 更新步骤：
- 通过选择 sigma 点来近似预测状态向量和预测协方差矩阵。
- 通过非线性测量函数 h 对 sigma 点进行测量，得到预测测量值的均值和协方差矩阵。
- 计算预测状态向量和预测测量值之间的协方差矩阵。
- 计算卡尔曼增益 K。
- 更新状态向量和协方差矩阵。