基础知识kalman方程
状态
1、Xk/k-1 = F Xk-1/k-1 + Gw
2、Pk/k-1 = F Pk-1/k-1 FT + Q
量测
3、ZK = H X+V
时间更新就是利用1、2方程中，k-1时刻的量得到k时刻的状态量
若要时间更新，需要知道状态转移矩阵A 以及系统噪声Q；
对应的INS代码
Fk = ++(Ft*tdts); // 获取状态转移矩阵 离散化后的结果
Xk = Fk*Xk; 对应方程1
Qk = Qt*tdts; // 获取系统噪声
**Pk = Fk*Pk*(~Fk); 对应方程2的前半部分
Pk += Qt*kfts;**添加系统噪声 对应方程2的后半部分
系统噪声Qk 中用到的是Qt与时间的乘积；QT的设置设计一个关键因素，后续再单独分析。
INS中未知数是各类误差，涉及到了误差方程（见下面的公式），Ft就是各个待估参数前面的系数。各个系数都有对应的计算方法，可参考相应的讲义，这里不在一一列举。
Fk = ++(Ft*tdts)中FK实际上是系数Ft离散化后的结果（ f_k = I+ft*ts）
离散过程参考严老师的讲义：
姿态误差微分方程
速度误差微分方程
位置误差微分方程
