INS量测更新

基础知识

1、ZK = H X +V  

2、V= ZK -HX

3、K = Pk/k-1HT/(HPk/k-1HT  +R) 

主要更新以下两个方程

4、Xk/k = Xk/k-1 +KV   （）

5、Pk/k  =  Pk/k-1  -  KHPk/k-1 = (I -KH)Pk/k-1;

剖析4和5两个方程，Xk/k-1,Pk/k-1时间更新已经更新完了，相当于已知。

那么需要知道KV和KH，K为增益矩阵，求解方式为3式，可以看出求解K也需要量测矩阵H，所以需要获得H矩阵。

式子1中 ZK是量测信息，        X是待估参数，H则是让两者连接起来的桥梁，直白的理解，就是如何让等式成立。

假如量测的方程ZK中的顺序是  Vn系 Pn系，X中的速度误差和位置误差也是n系下，则与之对应的H中的系数就是1；

假如ZK方程有m个，带估参数X有n个，则H矩阵的维数为  m*n;

获取V 方程2

代码中的小r 其实就是v = zk -hx

获取增益矩阵K 方程3，对应代码中的kk

kk = pxz/pzz =  pxz/( Pz0+R)  =  pk*ht /( h*pk*ht+R)

h前面已经求出,那么需要知道R，R是量测的方差协方差阵，例如gnss的速度 位置的误差
R一般会给个初值以后，自适应调整。