卡尔曼滤波（笔记）

卡尔曼滤波器

linear gaussian——>KF

week_nonlinear gaussian——>EKF

high_nonlinear,gaussian——>UKF

HIgh_nonlinear,non_gaussian——>PF

1.卡尔曼滤波浅层解读

解决问题

进行预测，先验知识和后验知识加权求和得到结果

假设小车在路上按照一定规律（能建立运动模型）行驶，测速仪可以测得t-1时刻车速，根据运动模型可预测出t时刻车速；也可以通过测速仪测量出车辆速度。

此时出现一个问题，相信预测or观测量，对测量值和预测值进行加权求和，确定下一状态。

总体思路：

分为时间更新和测量更新两过程：

时间更新方程：负责及时向前推算当前状态变量（计算预测值），和误差协方差估计值（Pk）,以便为下一个时间状态构造先验估计。

计算预测值Xk，并求预测值和真实值之间的误差协方差矩阵Pk

测量更新方程：根据先验估计和新的测量变量加权拟合

后验状态估计：作为（先验状态估计）和（实际测量和测量预测之间的加权差分）的线性组合

主要步骤

（1)

A：特征矩阵，常量，连接X（k-1）和X（k）

B：输入矩阵，变量，连接输入矩阵U（k-1）到状态量X（k）

(2) 计算先验估计

        (3)后验状态估计=先验状态估计+权重*（实际测量值-测量预测值）

    

        (4)计算增益（权重）

        

    （5）更新协方差（ 计算估计值和真实值之间的误差协方差矩阵，为下次递推做准备）

        

A：特征矩阵，常量，连接X（k-1）和X（k）

B：输入矩阵，变量，连接输入矩阵U（k-1）到状态量X（k）

(2) 计算先验估计

主要步骤

（1)

主要步骤

（1)