这里写自定义目录标题
前言
车载测试采集了MTI-G-710 Mems IMU，GNSS，光纤惯导的数据，MTI-G-710产品输出姿态角数据，现场测试图如下所示：
非常感谢严老师，他对数据进行了分析和组合导航算法调参，代码和数据可以从如下网站下载：
代码和数据
代码分析
代码运行需要严老师psins210406组合导航函数库的支持，这个开源组合导航函数库由严老师开发和维护。
主函数
主函数我做了大量的详细注释，如下所示：
glvs;
close all;
ts = 1/100;
t1 = 700; t2 = 1300;
%t1 = 700; t2 = 10000;
load lb_memsfoggps;
imuplot(imuFOG);
imuplot(imuMTI);
gpsplot(gps);
insplot(attMTI, 'a');% MTI产品输出的姿态角
% 惯性系对准{过程持续300s}
% 产品输出yaw初始对准：-53.084344000000002
% att0结果：-53.1423
att0 = aligni0(imuFOG(400/ts:t1/ts,:), gps(1,4:6)');
% 光纤惯导纯惯性解算
avp = inspure(imuFOG(t1/ts:t2/ts,:), [att0; getat(gps,t1)], 'H');
% 光纤惯导姿态角和MTI姿态角比较
avpcmpplot(avp(:,[1:3,end]), datacut(attMTI,t1,t2), 'a', 'datt');
avp0 = [att0; getat(gps,t1)];
ins = insinit(avp0, ts);
% 欧拉角初始误差：pitch:600角分,roll:600角分,偏航角：1800角分
% 速度初始误差：10m/s
% 位置初始误差：100m
avperr = avperrset([10*60;30*60], 10, 100);
% 设置IMU噪声，噪声含义如下：
% 陀螺仪偏置：1000deg/h (设置初始零偏，设置P矩阵)
% 加速度计偏置：10000ug[10mg] (设置初始零偏，设置P矩阵)
% 陀螺仪高斯噪声： 0.1deg/sqrt(Hz)
% 加速度计高斯噪声：100ug/sqrt(Hz)
imuerr = imuerrset(1000, 10000, 0.1, 100);
% 限制P矩阵(最小值)
% 欧拉角初始误差：pitch:0.5角分,roll:0.5角分,偏航角：2角分
% 速度初始误差：0.01m/s
% 位置初始误差：0.01m
% 陀螺零偏：1deg/h,1deg/h,1deg/h
% 加速度计零偏：100ug,100ug,100ug
% 杆臂(空间不同步误差)：0.01m
% 时间不同步误差：0.01s
Pmin = [avperrset([0.5,2],0.01,0.01); gabias(1, [100,100]); [0.01;0.01;0.01]; 0.01].^2;
% Rmin:观测噪声的最小值
% 速度初始误差：0.1m/s
% 位置初始误差：0.1m
Rmin = vperrset(0.1, 0.1).^2;
% rep3(1):lever协方差
% 0.1:dT协方差
% poserrset(10):GPS速度观测噪声：1m/s 1m/s 1m/s GPS位置观测噪声：10m 10m 10m
[avp1, xkpk1, zkrk1, sk] = sinsgps(imuMTI(t1/ts:t2/ts,:), gps, ins, avperr, imuerr, rep3(1), 0.1, vperrset(1,10), Pmin, Rmin, 'avped');
avpcmpplot(avp, avp1, 'avp', 'mu');% mems_sins/gnss组合导航和FOG数据比较姿态
avpcmpplot(gps, avp1, 'vp'); % mems_sins/gnss组合导航和gnss数据比较速度和位置
因为原始数据中有光纤惯导的加速度计数据和陀螺仪数据，所以代码中用光纤惯导解算出来的姿态角和方位角作为真值，因为GNSS大部分工作在RTK模式，所以速度和位置是精度比较高的，所以作为速度和位置的真值。
光纤惯导的初始对准严老师用了惯性系对准，具体方法可以参考严老师的博士后报告。通过和光纤惯导输出的初始方位角进行比较，差别大约为0.06deg。主函数接下来主要是设置卡尔曼滤波参数PQR，sinsgps函数是sins/gnss组合导航的核心函数。最后的avpcmpplot函数是通过和真值之间的对比，用于可视化组合导航的结果。
sinsgps核心函数
sinsgps函数的注释如下：
function [avp, xkpk, zkrk, ins, kf] = sinsgps(imu, gps, ins, davp, imuerr, lever, dT, rk, Pmin, Rmin, fbstr)
% 19-state SINS/GPS integrated navigation Kalman filter.
% The 19-state includes:
% phi(3), dvn(3), dpos(3), eb(3), db(3), lever(3), dT(1)
%
% Example1:
% [avp1, xkpk, zkrk, ins1, kf] = sinsgps(imu, gps, 300);
%
% Example2:
% ins = insinit([yaw;pos], ts);
% avperr = avperrset([60;300], 1, 100);
% imuerr = imuerrset(0.03, 100, 0.001, 1);
% Pmin = [avperrset([0.1,1],0.001,0.01); gabias(0.1, [10,30]); [0.01;0.01;0.01]; 0.0001].^2;
% Rmin = vperrset(0.001, 0.01).^2;
% [avp1, xkpk, zkrk, ins1, kf] = sinsgps(imu, gps, ins, avperr, imuerr, rep3(1), 0.01, vperrset(0.1,10), Pmin, Rmin, 'avp');
%
% See also kfinit, kfupdate
% Copyright(c) 2009-2021, by Gongmin Yan, All rights reserved.
% Northwestern Polytechnical University, Xi An, P.R.China
% 09/10/2013, 06/02/2021
% nn = 2
% ts = 0.01
% nts = 0.02
[nn, ts, nts] = nnts(2, diff(imu(1:2,end)));
if size(gps,2)<=5, gpspos_only=1; pos0=gps(1,1:3)'; else, gpspos_only=0; pos0=gps(1,4:6)'; end
if ~exist('rk', 'var')
% rk：GPS观测噪声(标准差)
if gpspos_only==1, rk=poserrset([10,30]);
else, rk=vperrset([0.1;0.3],[10,30]); end
end
if ~exist('dT', 'var'), dT = 0.01; end
if ~exist('lever', 'var'), lever = rep3(1); end
if ~exist('imuerr', 'var'), imuerr = imuerrset(0.01, 100, 0.001, 1); end
if ~exist('davp', 'var'), davp = avperrset([10;300], 1, [10;30]); end
if ~exist('ins', 'var'), ins=100; end
if ~isstruct(ins) % T=ins align time
[~, res0] = aligni0(imu(1:fix(ins/ts),:), pos0); % 惯性系对准
ins = insinit([res0.attk(1,1:3)'; 0;0;0; pos0], ts); ins.nts=nts;
end
psinstypedef(196-gpspos_only*3);
kf = [];
% kf.Qt：预测噪声
% 状态变量： 19状态：欧拉角,速度,位置,陀螺零偏,加速度计零偏,杆臂,时间不同步误差
% imuerr.sqg:一阶马尔科夫噪声，这里是0
% imuerr.sqa:一阶马尔科夫噪声，这里是0
% imuerr.web:陀螺仪高斯噪声
% imuerr.wdb:加速度计高斯噪声
% 预测噪声中只考虑了陀螺仪高斯噪声和加速度计高斯噪声
kf.Qt = diag([imuerr.web; imuerr.wdb; zeros(3,1); imuerr.sqg; imuerr.sqa; zeros(3,1); 0])^2;
% kf.Rk：观测噪声[GPS速度噪声,GPS位置噪声]
kf.Rk = diag(rk)^2;
% kf.Pxk：协方差矩阵设置
% davp：欧拉角初始误差：pitch:600角分,roll:600角分,偏航角：1800角分
% davp：速度初始误差：10m/s
% davp：位置初始误差：10m
% imuerr.eb：陀螺仪零偏
% imuerr.db：加速度计零偏
% rep3(lever)：杆臂
% dT：时间不同步误差
kf.Pxk = diag([davp; imuerr.eb; imuerr.db; rep3(lever); dT]*1.0)^2;
% kf.Hk：观测转换矩阵
% kfinit0：初始化KF结构体
kf.Hk = zeros(length(rk),19); % 初始化
% kfinit0：初始化KF结构体
kf = kfinit0(kf, nts);
% Pmax在kfinit0中赋值
if exist('Pmin', 'var')
if length(Pmin)==1, kf.pconstrain=0;
else, kf.Pmin = Pmin; kf.pconstrain = 1; end
end
% 自适应滤波：主要是对观测自适应
% Rmin和Rmax默认值在kfinit0中设置
kf.adaptive = 1;
if exist('Rmin', 'var')
if length(Rmin)==1, kf.adaptive=0; end
if kf.adaptive==1, kf.Rmin = diag(Rmin); end
end
% fbstr:反馈量
if exist('fbstr', 'var'), kf.fbstr=fbstr; end
kf.xtau = [ [1;1;1]; [1;1;1]; [1;1;1]; [1;1;1]; [1;1;1]; [1;1;1]; 1];
imugpssyn(imu(:,7), gps(:,end)); % imu,gps软件同步
len = length(imu); [avp, xkpk, zkrk] = prealloc(fix(len/nn), 16, 2*kf.n+1, 2*kf.m+1); % 预分配空间
timebar(nn, len, '19-state SINS/GPS simulation.'); ki = 1; kiz = 1;
for k=1:nn:len-nn+1
k1 = k+nn-1;
wvm = imu(k:k1,1:6); t = imu(k1,end);
ins = insupdate(ins, wvm); % imu前向预测过程
% kf.Phikk_1：计算状态转移矩阵
kf.Phikk_1 = kffk(ins);
kf = kfupdate(kf);
% 由imugpssyn的同步机制可以知道INS数据和GPS数据的时间同步误差最大为：惯导数据周期：T_sins
% 找到了两个惯导数据之间的GPS数据，否则：kgps=0
[kgps, dt] = imugpssyn(k, k1, 'F');
if kgps>0 % 如果有GPS数据
% sins数据和gps数据转到一个统一的坐标系{这里是GPS天线所在的坐标系}
ins = inslever(ins); % 杆臂补偿
% ins.vnL:杆臂补偿后的惯导速度（导航系）
% ins.tDelay一直等于0
% dtpos一直等于0
% fprintf("ins.tDelay = %.6f\n",ins.tDelay);
dtpos=+vn2dpos(ins.eth,ins.vnL,ins.tDelay);
if gpspos_only==1
zk = ins.posL+dtpos-gps(kgps,1:3)';
kf.Hk = [zeros(3,6), eye(3), zeros(3,6), -ins.MpvCnb,-ins.Mpvvn];
else
zk = [ins.vnL+ins.tDelay*ins.an;ins.posL+dtpos]-gps(kgps,1:6)';
% kf.Hk：观测矩阵
% eye(6)：速度和位置
% -ins.CW:杆臂引起的速度误差
% -ins.MpvCnb:杆臂误差引起的位置误差
% -ins.an：时间不同步误差引起的速度误差
% -ins.Mpvvn：时间不同步误差引起的位置误差
kf.Hk = [zeros(6,3), eye(6), zeros(6,6), [-ins.CW,-ins.an;-ins.MpvCnb,-ins.Mpvvn]];
end
kf = kfupdate(kf, zk, 'M');
zkrk(kiz,:) = [zk; diag(kf.Rk); t]; kiz = kiz+1;
end
[kf, ins] = kffeedback(kf, ins, nts);
avp(ki,:) = [ins.avp; ins.eb; ins.db; t]';
xkpk(ki,:) = [kf.xk; diag(kf.Pxk); t]'; ki = ki+1;
timebar;
end
avp(ki:end,:) = []; xkpk(ki:end,:) = []; zkrk(kiz:end,:) = [];
insplot(avp);
kfplot(xkpk);
% zkrk：
% 1-6:观测到的速度，位置误差
% 7-12:观测到的速度，位置方差
% 13:时间戳
rvpplot(zkrk);
IMU参数辨识
MTI-G-710手册标称参数如下：
使用艾伦方差辨识IMU噪声参数可以参考以下链接：
艾伦方差辨识IMU噪声参数
下图是MTI-G-710 Z轴陀螺仪艾伦方差：
容易得到：
零偏不稳定性：
B
=
1
8
∘
/
(
2
/
3
)
/
h
=
2
7
∘
/
h
B=18^{\circ} /(2 / 3) / h=27^{\circ} / h
B=18∘/(2/3)/h=27∘/h
可以看到比数据手册中标称的稍微大点。
角度随机游走：
N
=
σ
A
R
W
⋅
τ
60
=
σ
A
R
W
1
3600
h
=
3
5
∘
h
60
=
0.5
8
∘
h
N=\sigma_{A R W} \cdot \frac{\sqrt{\tau}}{60}=\sigma_{A R W} \sqrt{\frac{1}{3600} h}=\frac{\frac{35^{\circ}}{\sqrt{h}}}{60}=\frac{0.58^{\circ}}{\sqrt{h}}
N=σARW⋅60τ=σARW36001h=60h35∘=h0.58∘
下图是MTI-G-710 Z轴加速度计艾伦方差：
角度随机游走：
52
μ
g
H
z
52 \frac{\mu g}{\sqrt{H z}}
52Hzμg
零偏不稳定性：
17
u
g
(
10
s
)
17 u g(10 s)
17ug(10s)
结果分析
1.原始数据分析
GPS数据大部分工作在RTK固定解模式下，少部分工作在单点定位模式或者浮点解模式下，如下所示：
GPS不同模式下的速度和位置误差是不同的，通过分析观测误差，即可定量计算观测误差，进而设置不同的观测噪声，实现自适应卡尔曼滤波。
2.陀螺仪零偏估计
陀螺仪零偏估计结果如下所示：
可以看到各个轴的零偏差别比较大，零偏估计结果较为稳定，可以看到Y轴和Z轴估计出来的零偏比较大，需要注意的是由于组合导航中没有扣除零偏常量，所以这个零偏中包含零偏重复项和零偏稳定性两部分。通过对零偏常量的计算分析也可以证实这一点。
3.加速度计零偏估计
从上图可以看出，在大约1000s左右时刻，加速度计快速收敛，这个时间，车辆转了180deg，转弯可以提高加速度计零偏的可观测度。
4.自适应卡尔曼滤波
kfupdate函数中使用了自适应卡尔曼滤波，主要是自适应观测噪声，主要思想是通过分析观测速度差和位置差数据，调整观测噪声。并且对观测噪声做了限幅操作。下面是观测速度差和观测速度R噪声的图表。可以看到当观测速度差比较大时，R也随之增大，从而达到自适应的目的。
下面是完整的速度，位置观测误差以及对应的观测噪声R。
需要注意的是当观测噪声比较大时，滤波器的收敛速度会降低，具体可以参考严老师的博客，链接如下所示：
量测噪声自适应与收敛之间的矛盾
5.组合导航下的方位角分析
下图是组合导航估计出来的姿态角和方位角
下面是和光纤惯导解算出来的姿态角，方位角比较的误差，如下所示：
可以看到整个过程中有3次转弯，3次转弯过程中方位角逐渐收敛到真值，说明转弯可以增加方位角的可观测度。
6. 调参
6.1 P矩阵
加速度计零偏严老师设置的为10mg，如图是加速度计零偏对应的P矩阵的收敛情况：
可以看到收敛很快。
我将加速度计零偏改小到1mg，可以明显看到收敛速度变慢，如下图所示：
625
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
