【卡尔曼滤波器的初步认知】

粗略解释：利用多个维度的信息对下一刻的结果进行估计，需要考虑干扰以及误差的影响

1.现存在一个可观测信号，维度不定假设为N，参数来源不定，首先假设不同参数对结果的影响分布概率以及方差
2.计算不同维度参数之间的协方差，构建一个NN维的协方差矩阵A（表示信号预测的方向），以及不同维度信息构成的状态矩阵B（表示信号更新的状态）
3.假设存在矩阵F，使得现有状态通过F对下一时刻的状态B1进行预测，则F为状态预测矩阵,状态更新的方程为B1=FB,A1=FAF(T),A1为A的下一时刻估计值，F(T)为F的转置
4.对于一般实际环境，卡尔曼滤波过程中会存在其他物品或者环境带来的无关干扰，此刻需要考虑对此部分干扰的滤除，对于状态矩阵B1,假设存在控制矩阵C以及对应的控制向量c，使得Cc的值达到一个误差修正的状态，同时对预测方向的干扰也进行修正，假设存在干扰信息的协方差矩阵Q，完成对于误差干扰的修正，此时的状态更新方程为B1=FB+Cc A1=FAF(T)+Q，此时的方程满足高斯分布
5.以上的更新均建立在传感数据准确的地步，对于存在一定误差的传感器，同样需要一定的修正，假设传感器传递信息的偏移矩阵为H，则实际数据的分布同样满足高斯分布，分布方程为：u=HB，W=HAH(T)，u为分布的概率，W为分布的协方差。
6.下一时刻的预测值中会出现数据与预测结果B1的误差，而且均存在不确定性(可能会朝着误差变大的方向)，概率用协方差矩阵R表示，分布用Z表示。传感器以及预测都为高斯分布，可计算二者分布的联合概率作分析。
7.高斯分布相乘的结果分布为：
K=w1*(w1+w2)(-1)，w1和w2分别是不同高斯分布的协方差矩阵
U1=u1+K*(u2-u1)，u1，u2表示数据信息每个维度的均值
w1=w1-Kw1，
最后：状态方程以及卡尔曼增益的迭代公式如下：
B1=B+K1(Z-u)；
A1=A-K1HA;
K1=AH(T)(HAH(T)+R)(-1);

详细的公式推导可参考：
https://blog.csdn.net/heyijia0327/article/details/17487467?ops_request_misc=%257B%2522request%255Fid%2522%253A%2522165043335516780255225466%2522%252C%2522scm%2522%253A%252220140713.130102334…%2522%257D&request_id=165043335516780255225466&biz_id=0&utm_medium=distribute.pc_search_result.none-task-blog-2_blogtop_positive~default-2-17487467.nonecase&utm_term=%E5%8D%A1%E5%B0%94%E6%9B%BC%E6%BB%A4%E6%B3%A2&spm=1018.2226.3001.4450