卡尔曼滤波（笔记）
卡尔曼滤波器
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)