严恭敏老师PSINS工具箱学习笔记-5

比较纯惯导和DR解算

利用工具箱对test_SINS.m和test_DR.m整合，误差设置一致控制变量，代码如下。

%% loading data
close all
glvs
trj1 = trjfile('trj10ms.mat');
insplot(trj1.avp); subtitle('TURE');
trj = bhsimu(trj1, 1, 10, 3, trj1.ts);% Height simulation for barometric altimeter
%% data simulation 
inst = [3;60;10]*glv.min;  kod = 1;  qe = 0; dT = 0;  % od parameters
trjod = odsimu(trj, inst, kod, qe, dT, 0);subtitle('DR simulation');
%% imu error setting
imuerr = imuerrset(0.01, 50, 0.001, 5);
% imu = imuadderr(trjod.imu, imuerr);
imu1= imuadderr(trj.imu, imuerr);
% avp0 error setting
davp = avperrset([0.5;0.5;5], 0.1, [10;10;10]);
% davp = avperrset([60;0;60], 0, 0);
avp00 = avpadderr(trjod.avp0,davp);
avp01 = avpadderr(trj.avp0,davp);
%% calulation
dinst = [15;0;10]*glv.min; dkod = 0.05;
avpd = drpure([imu1(:,1:6), trjod.od], avp00, inst+dinst, kod*(1+dkod));subtitle('DR calculation');
avp = inspure(imu1, avp01, trj.bh, 1);subtitle('INS calculation');
insavperr = avpcmpplot(trj1.avp, avp);
dravperr = avpcmpplot(trj1.avp,[avpd(:,1:9),avpd(:,end)]);

trjsee(trj1.avp,avp,[avpd(:,1:9),avpd(:,end)])
legend('True trajectory', 'pure ins', 'DR');

1. 数据加载及dr数据仿真

可以看出DR使用imu和里程仪的数据，而ins仅使用imu数据进行解算。（dr使用的是odsimu仿真的加噪声数据，还是真实的imu加噪声后的数据，这里会影响失准角的误差）

2. 误差添加，同时影响DR和INS

误差类型	误差设置	含义	单位
IMU数据	eb	0.01	deg/h
	db	50	ug
	web	0.001	deg/√h
	wdb	5	ug/√Hz
初始AVP	姿态	[0.5,0.5,5]	min
	速度	0.1	m/s
	位置	[10 10 10]	m
DR误差	安装偏差角dinst	[15,0,10]	min
	刻度系数dkod	0.05	/

下图为使用真实imu加噪后的数据：

下图为使用仿真加噪后的数据：

使用真实imu加噪后的数据失准角误差与物体的姿态无关，使用仿真加噪后的数据失准角误差与物体的姿态有关且比较稳定，仿真的imu数据是根据ap信息解算出来的。
后面讨论使用仿真出的加噪imu数据。

3. 解算

可以看出ins的结果越来越偏离真实轨迹，另编写函数trjsee。

纯惯导avp误差：

DR的avp误差：

红色为DR，蓝色为INS：

对比纯惯导和DR的解算误差，可以看出：

• 在姿态解算方面，INS的失准角误差与初始设置[0.5,0.5,5]一致，而DR的失准角与里程仪的安装角偏差和物体的姿态相关（与姿态相关是因为使用的imu数据是仿真的数据）。
• 在速度解算方面，DR（±0.5m/s）明显优于INS。INS随时间增大，而DR也与安装角偏差和物体姿态相关。
• 在位置解算方面，从趋势来看INS的L和λ似乎呈振荡趋势，而DR的H和λ都较为平稳，L发散；在数据大小方面DR也优于INS，可以设置更长时长观察INS。

ps.INS对初始姿态很敏感，具体需要控制误差输入观察。

附：轨迹观察函数

function trjsee(avp,varargin)
avptrue=avp;
[avp1,avp2] = setvals(varargin);
myfigure;
plot(r2d(avptrue(:,8)), r2d(avptrue(:,7)),'LineWidth',2); xygo('lon', 'lat');
hold on, plot(r2d(avp1(:,8)), r2d(avp1(:,7)), 'r-','LineWidth',2);
hold on, plot(r2d(avp2(:,8)), r2d(avp2(:,7)), 'g-','LineWidth',2);
plot(r2d(avptrue(1,8)), r2d(avptrue(1,7)), 'rp');
legend('True trajectory', 'Calulated trajectory 1', 'Calulated trajectory 2','Location','Best');