mpu6500-gnss组合导航代码分析

---

前言

导航数据为如下链接，数据集使用了低成本Mems器件MPU6500和GNSS做组合导航。代码运行需要严老师psins210406组合导航函数库的支持，这个开源组合导航函数库由严老师开发和维护。

一组MPU6500-GNSS跑车数据

代码分析

主函数如下：

% 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矩阵变化如下：

从上图的变化中可以看到：

1. 陀螺偏置为10deg/h时，P矩阵一直在收敛的过程中，最终也没有完成收敛，零偏估计值也不正常，收敛速度较慢也影响了估计精度。
2. 陀螺偏置为100deg/h时，P矩阵最终达到了收敛，和10deg/h相比较好些，零偏估计值也逐渐接近真值，在700s以后零偏估计逐渐稳定。但是可以看到，收敛速度依然很慢。
3. 陀螺偏置为400deg/h时，可以看到P矩阵收敛较快，在前100s秒内，x，y轴已经达到了快速收敛。零偏估计值在100s之后又小幅度变动，但是没有明显突变，效果明显变好。
4. 陀螺偏置为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矩阵变化如下：

估计的零偏如下所示：

从上图的变化中可以看到：

1. 加速度计偏置为1mg时，P矩阵一直在收敛的过程中，最终也没有完成收敛，零偏估计值也收敛较慢。
2. 加速度计偏置为5mg时，P矩阵收敛明显加快，收敛的平滑性也很不错，同时可以看到在中间段，由于车辆存在转弯，所以增加了系统了可观测性，快速收敛到了真值。
3. 加速度计偏置为10mg时，P矩阵收敛速度进一步加快，但是也能看到零偏估计曲线的平滑性降低了一些。