文章目录

前言
代码分析
调参
- P矩阵
- - 陀螺仪偏置P矩阵
  - 加速度计偏置P矩阵

前言

导航数据为如下链接，数据集使用了低成本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 equalt0 = 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矩阵收敛速度进一步加快，但是也能看到零偏估计曲线的平滑性降低了一些。

mpu6500-gnss组合导航代码分析相关推荐

MTI-G-710/GNSS组合导航代码分析
这里写自定义目录标题前言代码分析主函数 sinsgps核心函数 IMU参数辨识结果分析 1.原始数据分析 2.陀螺仪零偏估计 3.加速度计零偏估计 4.自适应卡尔曼滤波 5.组合导航下的方位角 ...
基于PSINS工具箱的卡尔曼滤波与SINS/GNSS组合导航
文章目录卡尔曼滤波与SINS/GNSS组合导航典型的SINS/GNSS组合滤波 POS处理卡尔曼滤波与SINS/GNSS组合导航用于Kalman滤波的函数有: psinstypedef(nnm ...
INS/GNSS组合导航（三）松耦合、紧耦合、深度耦合
0 INS/GNSS组合导航概述所谓INS/GNSS组合导航,就是利用INS和GNSS两者信息进行融合,从而综合利用两者的优点,实现优势互补,求解实现pose的解算,得到PVA(位置.速度.姿态)的 ...
INS/GNSS组合导航（六）-惯性器件的主要误差
在SINS的各种误差源中,影响最大的是惯性传感器误差,这其中又包括加速度计和陀螺仪的误差.从误差的性质上可以分为系统误差和随机误差,而从误差源上又可以分为零偏误差.尺度因子误差.安装误差.非线性误差以 ...
INS/GNSS组合导航（十二）如何读懂MEMS惯性器件精度?
系列1跟大家解释了惯性器件中几种不同的陀螺零偏指标以及如何正确地对比器件性能.本系列我们以一款典型MEMS IMU模块为例来带着大家过一遍MEMS的常见精度指标,希望能对大家正确理解和选用MEMS惯性 ...
SINS/GNSS组合导航仿真应用详细版(基于PSINS工具箱 )
文章目录轨迹仿真生成轨迹数据绘制仿真数据惯导仿真纯惯导仿真 SINS/GPS组合导航总结 1 2 3 4 5 轨迹仿真生成轨迹数据首先,打开demos\test_trj.m文件,运行仿 ...
INS/GNSS组合导航（二）-INS常用的坐标系统及转换
一.常用坐标系由于INS是一种完全自主的导航系统,不与外界发生联系.但是我们又想知道物体的的位置,尤其是其与GNSS组合时需要知道其与GNSS在同一坐标系下的位置信息,这就牵扯到了INS的几个导航系 ...
SINS/GNSS组合导航：SINS误差模型
SINS导航精度会受到多种误差所带来的影响,其中主要受到三个方面误差所造成的影响,其一是在系统的组装过程中会受到来自硬件精度,安装误差所带来的影响:其二是捷联解方法造成的算法误差,例如初始值误差,对中 ...
INS/GNSS组合导航（七）-SINS的微分方程的推导
(三)中对SINS的机械编排进行了初步可行性的介绍,并未对机械编排进行原理性介绍.那么在详细介绍机械编排之前,需要先对SINS的微分方程进行详细的推导. 无论是机械编排,还是后面误差方程的建立,SIN ...

mpu6500-gnss组合导航代码分析

文章目录

前言

代码分析

调参

P矩阵

陀螺仪偏置P矩阵

加速度计偏置P矩阵

mpu6500-gnss组合导航代码分析相关推荐

最新文章

热门文章