PSINS函数解读之test_SINS_ZUPT

1.ZUPT概念理解

2.零速判定

3.PSINS ZUPT函数解读

3.1 INS初始化部分

如上图所示 首先设置IMU误差(零偏和随机游走；然后对纯净的IMU原始观测量加入零偏和随机游走误差；对初始AVP加入噪声；初始化INS时间更新函数(初始姿态，初始地球参数等）

3.2 卡尔曼滤波初始化

变量rk代表设置初始的速度和位置误差；

kf.Qt代表过程噪声，对失准角和速度误差部分加入陀螺和速度的随机游走；

kf.Rk 代表量测噪声，对预设的初始速度和位置误差作平方即可

kf.Pxk代表状态向量的方差阵，在零偏部分则是直接加入了零偏

kf.Hk代表量测矩阵，在不考虑杆臂误差的情况下(15维度的状态向量如下所示),如下图所示：

由于零速修正无位置观测量，所以在后续中 会将位置部分的1置为0；

kf.xk代表状态向量，在初始化时全部置为0

3.3进入卡尔曼滤波

首先是insupdate函数 实现INS姿态、速度、位置更新；

然后，在kffk中计算状态转移矩阵；

在kfupdate中进行时间更新（ins.vn=ins.vn-0,表示calculate minus observation 在ZUPT中将所有的状态待估参数置为0）；

紧接着大概600s一次，进行0速修正。将量测的速度置为惯导递推的速度；量测的R阵如下所示；

滤波完之后在kffeedback中进行全反馈。