前言
导航数据为如下链接,数据集使用了低成本Mems器件MPU6500和GNSS做组合导航。代码运行需要严老师psins210406组合导航函数库的支持,这个开源组合导航函数库由严老师开发和维护。
代码分析
主函数如下:
% gps文件格式:
% 第一列 时间
% 第2~4列 纬经高
% 第5~7列 位置信息标准差
%
% imu文件格式:
% 第一列:时间
% 第2~4列:加速度计数据 单位g
% 第5~7列:陀螺数据 单位°/s
% IMU朝向:后右上
% 杆臂:三个方向都是0
close all;
glvs;
ts = 1/100;
dd = load('imu.txt');
t0 = dd(1,1);
% figure;
% plot(diff(dd(:,1)),'b-*');
% title('dif-time');
imu = imurfu(adddt(imuidx(dd, [5:7, 2:4, 1], glv.dps, glv.g0, ts),-t0),'bru');
% [1380,1400]:静止状态
imu = imudeldrift(imu, 1380,1400); imuplot(imu);
dd1 = load('gps.txt');
gps = adddt(gpsidx(dd1, [2:4, 1], 0),-t0); gpsplot(gps);
figure;
plot(diff(gps(:,1)),'b-*');
title('dif-time');
pos2dplot(gps); axis equal
t0 = 1;
% 初始位姿:静止,速度为0,GPS没有速度观测
avp0 = [[0;0;-100]*glv.deg; 0;0;0; getat(gps,t0)];
ins = insinit(avp0, ts);
% 欧拉角初始误差:pitch:600角分,roll:600角分,偏航角:1800角分
% 速度初始误差:10m/s
% 位置初始误差:10m
avperr = avperrset([10*60;30*60], 10, 100);
% avperr = avperrset([10*60;30*60], 10, 10);
% 设置IMU噪声,噪声含义如下:
% 陀螺仪偏置:100deg/h (设置初始零偏,设置P矩阵)
% 加速度计偏置:10000ug[10mg] (设置初始零偏,设置P矩阵)
% 陀螺仪高斯噪声: 1deg/sqrt(Hz)
% 加速度计高斯噪声:100ug/sqrt(Hz)
imuerr = imuerrset(500, 10000, 10, 100);
% imuerr = imuerrset(500, 5000, 5, 500); % 严老师:优化
% imuerr = imuerrset(1005, 10005, 110, 10010); % 陈宇鸣 中国矿业大学设置参数
% 限制P矩阵(最小值)
% 欧拉角初始误差:pitch:0.5角分,roll:0.5角分,偏航角:3角分
% 速度初始误差:0.1m/s
% 位置初始误差:0.1m
% 陀螺零偏:0.1deg/h,0.1deg/h,0.1deg/h
% 加速度计零偏:100ug,100ug,100ug
% 杆臂(空间不同步误差):0.01m
% 时间不同步误差:0.001s
Pmin = [avperrset([0.5,3],0.1,0.1); gabias(0.1, [100,100]); [0.01;0.01;0.01]; 0.001].^2;
% Rmin:观测噪声的最小值
% 位置初始误差:0.01m
Rmin = poserrset(0.1).^2;
% rep3(1):lever协方差
% 0.1:dT协方差
% poserrset(10):GPS位置观测噪声:10m 10m 10m
[avp1, xkpk, zkrk, ins1, kf] = sinsgps(imu(1:1000/ts,:), gps, ins, avperr, imuerr, rep3(1), 0.1, poserrset(10), Pmin, Rmin, 'avped');
avp2 = insinstant(imu, avp1, 420, 430);
avpcmpplot(gps, adddt(avp1(:,[7:9,end]),0.0), 'p');、
调参
P矩阵
陀螺仪偏置P矩阵
首先测试P矩阵中的零偏对组合导航的影响,角度随机游走可以通过艾伦方差辨识得到。
设置参数如下:
陀螺仪偏置设置的比较小,为10deg/h。
imuerr = imuerrset(10, 10000, 10, 100);
陀螺仪零偏P矩阵变化如下:
估计的零偏如下所示:
陀螺仪偏置设置的大一些,为100deg/h。
imuerr = imuerrset(100, 10000, 10, 100);
陀螺仪零偏P矩阵变化如下:
估计的零偏如下所示:
陀螺仪偏置设置再大一些,为400deg/h。
imuerr = imuerrset(400, 10000, 10, 100);
估计的零偏如下所示:
陀螺仪偏置设置再大一些,为10000deg/h。
imuerr = imuerrset(10000, 10000, 10, 100);
估计的零偏如下所示:
陀螺仪零偏P矩阵变化如下:
从上图的变化中可以看到:
- 陀螺偏置为10deg/h时,P矩阵一直在收敛的过程中,最终也没有完成收敛,零偏估计值也不正常,收敛速度较慢也影响了估计精度。
- 陀螺偏置为100deg/h时,P矩阵最终达到了收敛,和10deg/h相比较好些,零偏估计值也逐渐接近真值,在700s以后零偏估计逐渐稳定。但是可以看到,收敛速度依然很慢。
- 陀螺偏置为400deg/h时,可以看到P矩阵收敛较快,在前100s秒内,x,y轴已经达到了快速收敛。零偏估计值在100s之后又小幅度变动,但是没有明显突变,效果明显变好。
- 陀螺偏置为1000deg/h时,可以看到P矩阵收敛较快,在前20s秒内,x,y,z轴已经达到了快速收敛。但是在前20s的零偏估计值曲线不太平滑,说明这段时间的零偏估计值不准确,也说明了初始P值设置过大导致的。
说明较大的P矩阵,可以计算得到较大的增益,可以加快P矩阵的收敛,收敛的速度不能太慢也不能太快,P矩阵的收敛和收敛的精度也有关系。
加速度计偏置P矩阵
.加速度计偏置设置的比较小,为1mg。
imuerr = imuerrset(500, 1000, 10, 100);
零偏P矩阵变化如下:
估计的零偏如下所示:
加速度计偏置设置的大些,为5mg。
imuerr = imuerrset(500, 5000, 10, 100);
零偏P矩阵变化如下:
估计的零偏如下所示:
加速度计偏置设置的大些,为10mg。
imuerr = imuerrset(500, 10000, 10, 100);
零偏P矩阵变化如下:
估计的零偏如下所示:
从上图的变化中可以看到:
- 加速度计偏置为1mg时,P矩阵一直在收敛的过程中,最终也没有完成收敛,零偏估计值也收敛较慢。
- 加速度计偏置为5mg时,P矩阵收敛明显加快,收敛的平滑性也很不错,同时可以看到在中间段,由于车辆存在转弯,所以增加了系统了可观测性,快速收敛到了真值。
- 加速度计偏置为10mg时,P矩阵收敛速度进一步加快,但是也能看到零偏估计曲线的平滑性降低了一些。