KF/UKF/EKF

UKF和EKF和粒子滤波
卡尔曼滤波（Kalman Filter，KF）是一种递归状态估计算法，它结合了观测数据和系统动态模型来估计系统状态。卡尔曼滤波广泛应用于导航、定位、控制等领域。

扩展卡尔曼滤波（Extended Kalman Filter，EKF）和无迹卡尔曼滤波（Unscented Kalman Filter，UKF）是卡尔曼滤波的扩展，用于解决非线性系统的状态估计问题。

粒子滤波（Particle Filter，PF）是一种基于蒙特卡洛方法的递归贝叶斯滤波器，适用于非线性非高斯系统的状态估计。

卡尔曼滤波（KF）：
KF的基本思想是将系统的状态和观测建模为线性高斯系统。KF包括两个步骤：预测和更新。
预测步骤：
x^_k = F_k * x_k-1
P^_k = F_k * P_k-1 * F_k^T + Q_k
更新步骤：
K_k = P^_k * H_k^T * (H_k * P^_k * H_k^T + R_k)^-1
x_k = x^_k + K_k * (z_k - H_k * x^_k)
P_k = (I - K_k * H_k) * P^_k

扩展卡尔曼滤波（EKF）：
EKF通过将非线性系统线性化来解决非线性问题。线性化是通过在当前状态估计点附近计算雅可比矩阵来完成的。
预测步骤：
x^_k = f(x_k-1)
F_k = df/dx(x_k-1)
P^_k = F_k * P_k-