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

惯性传感器误差模型

前提是：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));

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

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

Psins代码解析之静基座仿真（test_SINS_static.m）傅科、修拉周期水平通道误差传播（test_SINS_static_verify.m）
静基座条件下:速度矢量为0:地理位置精确已知: 一.静基座下,陀螺仪和加速度计数据仿真: imustatic.m 重力矢量和地球自转角速度在地理坐标系下: 上式中,Wnen为0,Wnnb为0,即静基座 ...
Psins代码解析之kalman松组合导航融合算法 test_SINS_GPS_153.mtest_SINS_GPS_186.mtest_SINS_GPS_193.m
框架: 设置松组合导航算法中状态量.观测量数目:比如:psinstypedef(153),状态误差量为15维,量测量为3维: 对仿真生成的飞行轨迹(test_SINS_trj.m),设置IMU误差:并 ...
Psins代码解析之全局变量轨迹仿真（test_SINS_trj.m）惯性解算（test_SINS.m）
旋转椭球体的4个基本参数:长半轴.扁率(椭圆度).地心引力常数.自转角速率: 以上内容来自:<车载定位定向系统关键技术研究>付强文旋转椭球体: 地球自转角速度: 地球重力加速度为: 子午 ...
Psins代码解析之常用的子函数
1.insinit.m 惯导参数结构体初始化(SINS structure array initialization). % Prototype: ins = insinit(avp0, ts, va ...
Psins代码解析之test_SINS_east_west.mtest_SINS_north_south.m
导航坐标系:东-北-天载体坐标系:右-前-上欧拉角定义:3-1-2旋转,(航向角-俯仰角-滚转角),其中航向角北偏西为正,范围[-pi pi] 地球自转引起的导航系旋转: 因地球表面弯曲,载体在地 ...
php精度两位小数点,php中小数精度的代码解析
本篇文章给大家带来的内容是关于php中小数精度的代码解析,有一定的参考价值,有需要的朋友可以参考一下,希望对你有所帮助. 项目中保留两位小数四舍五入遇到精度问题:$num = 0.99; $num1 ...
hdl_localization代码解析
hdl_localization代码解析简介 hdl_localization是基于UKF滤波框架,融合了ndt点云配准结果,在已经构建的点云地图上实习激光重定位的一种方法.在使用16线激光雷达进行 ...
EfficientNet（ICML 2019）原理与代码解析
paper:EfficientNet: Rethinking Model Scaling for Convolutional Neural Networks code:mmclassification ...
代码解析之自行车模型在Apollo规划中的应用
大家好,我已经把CSDN上的博客迁移到了知乎上,欢迎大家在知乎关注我的专栏慢慢悠悠小马车(https://zhuanlan.zhihu.com/duangduangduang).希望大家可以多多交流, ...

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

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

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

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

最新文章

热门文章