EKF的理解

若已知参考点（landmarks）的坐标，则状态向量中不必含有x_L， 从而实现的仅为机器人在已知环境中的定位，求解大大减少（状态向量维度仅为运动状态）。若想实现完整SLAM，必须将x_L加入状态向量中。

扩展卡尔曼滤波（EKF）相对于卡尔曼滤波，可以进一步求解非线性问题（通过在目标点附近做泰勒展开的一级近似），但是依然建立在输入噪声和测量噪声均为高斯的前提下。高斯噪声的好处是它的e指数形式使得高斯与高斯的卷积、乘法结果依然是高斯，从Bayes理论推导出的EKF结果中，我们只用计算高斯分布的期望mu和协方差sigma）

---

n 维扩展卡尔曼滤波

1. Prediction (motion) model:

x_t = A_tx_t-1 + B_tu_t + ε_R

x_t：t时刻，n维状态空间向量；

x_t-1：t-1时刻，n维空间状态向量；

u_t：t时刻输入，将状态从x_t-1变换到x_t；

ε_R：输入误差。

依据motion model，我们可以得出期望和协方差满足：

μ_t（prior）= A_tμ_t-1 + B_tu_t

Σ（prior）= A_tΣ_t-1A_t^T + R_t

对于线性模型，直接带入A_t B_t R_t就行，而对于非线性模型，我们需要求运动方程对输入（V = ）的导数，或者运动方程对状态（G = ）的导数。

例如对于控制左右两排车轮速度（r，l）从而实现小车状态（x，y，θ）改变的运动方程，At为3*3矩阵，Bt为3*2矩阵，Rt为3*3矩阵，对于Rt，我们是不能直接知道的，我们只知道r和l的误差，得到的是2*2的协方差矩阵，通过VΣ_controlV^T可以得到相应的3*3矩阵