Psins代码解析之线性误差模型精度验证(test_SINS_error_model_verify.m)

捷联惯导误差方程的推导前提是：

• 惯性传感器误差模型

前提是：IMU设备经过出厂标定之后，对所谓IMU传感器存在的“残差”的考虑；主要是：失准角、非正交角、零偏、刻度系数、二次项误差等；

• 姿态误差方程

假定载体坐标系是重合的， 理想导航坐标系和计算导航坐标系之间存在失准角；基于失准角旋转矢量，对姿态微分方程进行推导，得到姿态误差方程；

• 速度误差方程

描述和姿态误差方程推导一致；

• 位置误差方程

描述和姿态误差方程推导一致；

通过以上可以看出，IMU由于自身存在缺陷（各种误差源），导致输出的角速率、加速度的数据内含误差；又因为捷联惯导的姿态、速度、位置方程是相互耦合的，即”我中有你，你中有我“，因此，在积分推算的过程中，误差会逐渐增大；从其本质上看，误差的缠身就是IMU自身的缺陷、以及数字计算机计算精度和捷联惯导算法精度等所导致。

一、SINS线性误差模型精度验证

需要指出的是：在误差模型中，只考虑了误差量的一阶小量，忽略误差量的二阶小量；忽略了重力异常、重力偏差；忽略其他参数（根据载体机动情况而定）

详细误差方程见：捷联惯导基础知识解析之二（捷联惯导更新算法和误差方程）

1、psinstypedef(346)  即状态为34，量测为6

采用双子样解算、设置IMU误差源、设置初始姿态、速度、位置误差；

% initial settings
psinstypedef(346);
[nn, ts, nts] = nnts(2, trj.ts); %双子样
imuerr = imuerrset(0.01, 100, 0, 0, 0,0,0,0, 10, 10); %初始零偏和比例因子误差(ppm)
imu = imuadderr(trj.imu, imuerr);
davp0 = avpseterr([10;-10;30], 0.1, [10;10;30]);
ins = insinit(avpadderr(trj.avp0,davp0), ts);

（1）纯惯导解算：

    k1 = k+nn-1;  
    wvm = imu(k:k1,1:6);  t = imu(k1,end);
    ins = insupdate(ins, wvm);    ins = inslever(ins);

（2）采用346方式的线性误差传递方程：

首先将误差状态(34个)赋初值：其中初值就是添加的误差项

xk = [davp0; imuerr.eb; imuerr.db; zeros(4,1); imuerr.dKga];  % init state error 34个状态 3+3+3+3+3+4+15=34

然后利用kalman滤波的状态预测方程和量测预测方程，对姿态、速度、位置误差进行预测：

最优一步估计：

量测一步预测：

代码如下：

    Fk = kffk(ins); Hk = kfhk(ins); Hk(1:3,1:3) = -askew(ins.vn); % ??
    xk = Fk*xk; zk = Hk*xk;
    avp(ki,:) = [ins.avp', t]; %纯惯导更新
    xkk(ki,:) = [xk(1:9)', t]; %kalman预测方程，预测更新
    zkk(ki,:) = [zk', t]; ki = ki+1; %kalman量测预测方程

（3）画图、误差传递方程精度验证

纯惯导解算：姿态、速度、位置误差

avperr = avpcmp(avp, trj.avp);
inserrplot(avperr);

姿态、速度、位置误差方程，误差状态传递求解 减去 纯惯导解算姿态、速度、位置参数得如下图：

inserrplot(avpcmp(xkk, avperr));

总结：两者相减误差比较小，可以忽略，验证误差传递方程的正确性！