基础知识

image.png
对当前小车状态进行评估，p代表位置，u代表速度
期望值 协方差
image.png
当小车匀速运动（a=0 速度不发生变化）时，已知k-1时刻的位置p、速度V，计算k时刻的**位置p、速度V。
image.png
当小车收到外力作用，p、u值根据加速度公式计算**

用矩阵形式表示

image.pngimage.png

噪声

image.png
W 为预估噪声 Q为观测噪声
image.png
H需要标定，用传感器去测量一个已知的运动过程，就能得到这个数。
image.png
预测的变量分布X（预测观测值）和测量出的变量分布Y（真实观测值）。

卡尔曼滤波如何工作

image.png
找到预测观测值和真实观测值的重合部分，最优估计

计算公式推导

image.png
image.png
image.png
最后协方差更新错了 应该是Pk’=Pk-K’*Pk
image.png

程序怎么写

image.png
image.png