卡尔曼滤波 Kalman Filter（笔记）

一、算法：

P：车的位置；
U:速度；
圈：p和u组合的可能分布；
中间的点：mean—期望—中间点的概率密度比较大，外面一圈的比较小

车的状态表达成矩阵的形式，pk为协方差矩阵；

1、
式1：下一状态的位置=上一状态位置+u*t
式2：匀速状态

2、加速度运动的公式

3、应卡尔曼滤波算法，需将匀速和加速度情况下的式子转换成矩阵，如下：

式1：此过程为参数估计的过程；其中，Fk为状态转移矩阵；
式2：Bk为控制矩阵࿱