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

痛定思痛。 2023-07-23 03:36 10阅读 0赞

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

*  **惯性传感器误差模型**

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

*  **姿态误差方程**

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

*  **速度误差方程**

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

*  **位置误差方程**

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

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

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

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

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

### **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滤波的状态预测方程和量测预测方程，对姿态、速度、位置误差进行预测：

最优一步估计：

![20200406174530886.png][]

量测一步预测：

![20200406174723576.png][]

代码如下：

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);

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3d1d3VrdTEyMw_size_16_color_FFFFFF_t_70][]

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

inserrplot(avpcmp(xkk, avperr));

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3d1d3VrdTEyMw_size_16_color_FFFFFF_t_70 1][]

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

[Link 1]: https://blog.csdn.net/wuwuku123/article/details/105244541
[20200406174530886.png]: /images/20230528/bef0b88978474fadb4ddc274f5ac5966.png
[20200406174723576.png]: /images/20230528/2047b34a30b24b40a5b1e7a8234b9edc.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3d1d3VrdTEyMw_size_16_color_FFFFFF_t_70]: /images/20230528/739abc5c873d465490202ce50311cc5a.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3d1d3VrdTEyMw_size_16_color_FFFFFF_t_70 1]: /images/20230528/fa31acb924414d719c2da311c966cb64.png