扩展卡尔曼滤波
erf
用于估计系统的状态，比如机器人的位姿、速度等信息，从而实现自主导航和控制
扩展卡尔曼滤波（Extended Kalman Filter，EKF）是卡尔曼滤波的一种扩展形式，主要用于非线性系统的状态估计。与标准卡尔曼滤波不同的是，EKF使用非线性函数来预测和更新状态变量，因此需要通过一定的线性化处理来近似估计非线性函数的导数，以便于卡尔曼滤波的计算。
EKF的主要思路是将非线性系统模型用线性化的方式来处理。具体来说，EKF将非线性的动力学和测量方程在估计点处进行一阶泰勒展开，得到线性近似模型。然后使用卡尔曼滤波的方法对线性近似模型进行处理，得到对状态变量的最优估计。
EKF的步骤如下：
-
根据非线性系统的动力学方程和测量方程，计算出预测状态和预测协方差矩阵。
-
对非线性函数进行泰勒展开，得到其在预测状态处的一阶导数，从而得到近似线性的状态转移矩阵和测量矩阵。
-
利用线性近似模型，使用卡尔曼滤波的方法进行状态更新，得到最优状态估计和最优协方差矩阵。
-
重复进行预测和更新步骤，得到连续的状态估计和协方差矩阵。
相对于标准的卡尔曼滤波，EKF可以更好地处理非线性系统，并且具有较好的估计精度。但是，EKF在处理高度非线性的系统时，由于线性化误差的累积，可能会导致估计的不准确，因此需要一定的经验和技巧来选择合适的状态变量和观测变量，以及合适的线性化方法，以确保EKF的正确性和稳定性。