自动驾驶-用无损卡尔曼滤波算法定位

前边章节介绍了卡尔曼滤波的原理，并用扩展卡尔曼滤波实现匀速直线运动模型的车辆定位。从上章的介绍了解到，扩展卡尔曼滤波的方式要用到非线性的转移函数和对该函数求偏导得到的雅可比矩阵。如果转移函数比较复杂，那么求解雅可比矩阵计算量也很大，为了应对这种情况，无损卡尔曼滤波应运而生。

下图给出的是车辆转弯的模型，相比匀速直线运动，这一模型加入了向角$\psi$和航向角速度$\dot{\psi }$；为了使模型更接近实际情况，在计算状态向量时还考虑到加速度$a$和航向角加速度$\ddot{\psi }$对状态向量的影响。

状态方程：
State = x k + 1 = x k + [ v k ψ ˙ k ( s i n ( ψ k + ψ ˙ k Δ t ) − s i n ( ψ k ) ) v k ψ ˙ k ( − c o s ( ψ k + ψ ˙ k Δ t ) + c o s ( ψ k ) ) 0 ψ ˙ k Δ t 0 ] + [ 1 2 ( Δ t ) 2 c o s ( ψ k ) ν a , k 1 2 ( Δ t ) 2 s i n ( ψ k ) ν a , k Δ t ⋅ ν a , k 1 2 ( Δ t ) 2 ν ψ ¨ , k Δ t ⋅ ν ψ ¨ , k ] ​ \text{State}= x_{k+