前言
高精度惯导的初始对准一般包括粗对准和精对准两个过程，粗对准是一个解析计算的过程，精对准一般使用卡尔曼滤波来实现，本文测试了使用了捷联惯导导航系速度作为观测的精对准算法，其本质相当于一个零速修正的过程，可以估计出陀螺和加表的零偏。最后分析了精对准系统的可观性。
使用的数据集的百度网盘链接如下：
提取码：4ik8
代码
主函数代码如下：
close all;
% 精对准算法测试：
% 车载测试数据
% 静止状态：0-670s
load lb_memsfoggps_ppp_20210414020704;
glvs
ts = mean(diff(imuFOG(:,end)));
t0 = 100; t1 = 300; t2 = 520;
att0 = aligni0(imuFOG(t0/ts:t1/ts,:), gps(1,4:6)');
att2_ref = aligni0(imuFOG(t1/ts:t2/ts,:), gps(1,4:6)'); % 参考
avp0 = [att0; getat(gps,t1)];
idx = (t1/ts:t2/ts)';
%% vn-meas. Kalman filter
% 初始姿态角，方位角
% 设置失准角P矩阵
phi = [.5;.5;5]*glv.deg;
% 设置IMU噪声，噪声含义如下：
% 陀螺仪偏置：0.5deg/h (设置初始零偏，设置P矩阵)
% 加速度计偏置：500ug[1mg] (设置初始零偏，设置P矩阵)
% 陀螺仪高斯噪声： 0.01deg/sqrt(h)
% 加速度计高斯噪声：25ug/sqrt(Hz)
imuerr = imuerrset(0.5, 500, 0.01, 25);
% 观测速度噪声 ：单位0.1m/s
wvn = [0.1;0.1;0.1];
[att0v, attkv, xkpkv] = alignvn2(imuFOG(t1/ts:t2/ts,:), att0, avp0(7:9), phi, imuerr, wvn, ts,att2_ref);
% myfigure;
% subplot(421); plot(t, attk(:,1:2)/glv.deg); xygo('pr'); title('姿态角'); % 姿态角
% hold on;
%
% subplot(423); plot(t, attk(:,3)/glv.deg); xygo('y'); title('方位角');% 方位角
alignvn函数如下所示：
function [att0, attk, xkpk] = alignvn2(imu, qnb, pos, phi0, imuerr, wvn, ts,att_ref)
% SINS initial align uses Kalman filter with vn as measurement.
% Kalman filter states:
% [phiE,phiN,phiU, dvE,dvN,dvU, ebx,eby,ebz, dbx,dby,dbz]'.
%
% Prototype: [att0, attk, xkpk] = alignvn(imu, qnb, pos, phi0, imuerr, wvn, ts)
% Inputs: imu - IMU data
% qnb - coarse attitude quaternion
% pos - position
% phi0 - initial misalignment angles estimation
% imuerr - IMU error setting
% wvn - velocity measurement noise (3x1 vector)
% ts - IMU sampling interval
% Output: att0 - attitude align result
% attk, xkpk - for debug
%
% Example:
% avp0 = [[0;0;0], zeros(3,1), glv.pos0];
% imuerr = imuerrset(0.03, 100, 0.001, 10);
% imu = imustatic(avp0, 1, 300, imuerr);
% phi = [.5; .5; 5]*glv.deg;
% wvn = [0.01; 0.01; 0.01];
% [att0, attk, xkpk] = alignvn(imu, avp0(1:3)', avp0(7:9)', phi, imuerr, wvn);
%
% See also alignfn, alignfn9, aligncmps, aligni0, alignWahba, alignsb, insupdate, etm.
% Copyright(c) 2009-2014, by Gongmin Yan, All rights reserved.
% Northwestern Polytechnical University, Xi An, P.R.China
% 17/06/2011
global glv
if nargin<4, phi0 = [1.5; 1.5; 3]*glv.deg; end
if nargin<5, imuerrset(0.01, 100, 0.001, 1); end
if nargin<6, wvn = [0.01; 0.01; 0.01]; end
if nargin<7, ts = imu(2,7)-imu(1,7); end
if length(qnb)==3, qnb=a2qua(qnb); end %if input qnb is Eular angles.
nn = 2; nts = nn*ts;
len = fix(length(imu)/nn)*nn;
% Cnn?
eth = earth(pos); vn = zeros(3,1); Cnn = rv2m(-eth.wnie*nts/2);
% 滤波器初始化
kf = avnkfinit(nts, pos, phi0, imuerr, wvn);
[attk, xkpk] = prealloc(fix(len/nn), 4, 2*kf.n);
ki = timebar(nn, len, 'Initial align using vn as meas.');
for k=1:nn:len-nn+1
wvm = imu(k:k+nn-1,1:6);
% phim：角度增量
% dvbm：速度增量
[phim, dvbm] = cnscl(wvm);
Cnb = q2mat(qnb);
% 注意：不进行位置更新！！！
% 速度更新方程
dvn = Cnn*Cnb*dvbm;
vn = vn + dvn + eth.gn*nts;
%qnb = qupdt(qnb, phim-Cnb'*eth.wnin*nts);
% 四元数更新方程
qnb = qupdt2(qnb, phim, eth.wnin*nts);
Cnbts = Cnb*nts;
% kf.Phikk_1:状态转移矩阵[时变矩阵]
kf.Phikk_1(4:6,1:3) = askew(dvn);
% 陀螺零偏转到导航坐标系
kf.Phikk_1(1:3,7:9) = -Cnbts; % 参考公式（7.2.3）
% 加表零偏转到导航坐标系
kf.Phikk_1(4:6,10:12) = Cnbts; % 参考公式（7.2.3）
% 卡尔曼滤波核心函数
kf = kfupdate(kf, vn);
% 状态向量更新
qnb = qdelphi(qnb, 0.1*kf.xk(1:3)); % 部分反馈校正
kf.xk(1:3) = 0.9*kf.xk(1:3);
vn = vn-0.1*kf.xk(4:6);
kf.xk(4:6) = 0.9*kf.xk(4:6);
attk(ki,:) = [q2att(qnb)',imu(k+nn-1,end)];
xkpk(ki,:) = [kf.xk; diag(kf.Pxk)];
ki = timebar;
end
attk(ki:end,:) = []; xkpk(ki:end,:) = [];
att0 = attk(end,1:3)';
resdisp('Initial align attitudes (arcdeg)', att0/glv.deg);
avnplot(nts, attk, xkpk,att_ref);
function kf = avnkfinit(nts, pos, phi0, imuerr, wvn)
eth = earth(pos); wnie = eth.wnie;
kf = []; kf.s = 1; kf.nts = nts;
% kf.Qt：预测噪声
% 状态变量： 12状态：失准角,速度,陀螺零偏,加速度计零偏
% imuerr.sqg:一阶马尔科夫噪声，这里是0
% imuerr.sqa:一阶马尔科夫噪声，这里是0
% imuerr.web:陀螺仪高斯噪声
% imuerr.wdb:加速度计高斯噪声
kf.Qk = diag([imuerr.web; imuerr.wdb; zeros(6,1)])^2*nts;
kf.Gammak = 1;
kf.Rk = diag(wvn)^2;
% kf.Pxk：协方差矩阵设置
% phi0：失准角：phi = [.5;.5;5]*glv.deg;
% [1;1;1]：速度初始误差：1m/s
% imuerr.eb：陀螺仪零偏
% imuerr.db：加速度计零偏
kf.Pxk = diag([phi0; [1;1;1]; imuerr.eb; imuerr.db])^2;
Ft = zeros(12);
Ft(1:3,1:3) = askew(-wnie); % 参考公式（7.2.3）
% 连续域到离散域
kf.Phikk_1 = eye(12)+Ft*nts;
kf.Hk = [zeros(3),eye(3),zeros(3,6)]; % 速度观测【不作差】
[kf.m, kf.n] = size(kf.Hk);
kf.I = eye(kf.n);
kf.xk = zeros(kf.n, 1);
kf.adaptive = 0; % 不自适应卡尔曼滤波
kf.xconstrain = 0;
kf.pconstrain = 0;
kf.fading = 1;
function avnplot(ts, attk, xkpk,att_ref)
global glv
t = (1:length(attk))'*ts;
myfigure;
subplot(421); plot(t, attk(:,1:2)/glv.deg); xygo('pr'); title('姿态角'); % 姿态角
hold on; plot(t(end),att_ref(1:2)'/glv.deg,'k*');
subplot(423); plot(t, attk(:,3)/glv.deg); xygo('y'); title('方位角');% 方位角
hold on; plot(t(end),att_ref(3)'/glv.deg,'k*');
subplot(425); plot(t, xkpk(:,7:9)/glv.dph); xygo('eb'); title('状态向量：陀螺零偏误差'); % 状态向量：陀螺零偏误差
subplot(427); plot(t, xkpk(:,10:12)/glv.ug); xygo('db'); title('状态向量：加表零偏误差'); % 状态向量：加表零偏误差
subplot(422); plot(t, sqrt(xkpk(:,13:15))/glv.min); xygo('phi'); title('P矩阵：失准角');% P矩阵：失准角
subplot(424); plot(t, sqrt(xkpk(:,16:18))); xygo('dV'); title('P矩阵：速度误差'); % P矩阵：速度误差
subplot(426); plot(t, sqrt(xkpk(:,19:21))/glv.dph); xygo('eb'); title('P矩阵：陀螺零偏'); % P矩阵：陀螺零偏
subplot(428); plot(t, sqrt(xkpk(:,22:24))/glv.ug); xygo('db'); title('P矩阵：加表零偏'); % P矩阵：加表零偏
结果
粗对准的结果如下：
-6.0147 1.0430 -51.9448
精对准的结果如下：
-6.0120 1.0445 -51.9222
零偏估计结果如下：
对于加表来说，确实可以通过P矩阵看到E,N是不可观的，U是可观测的，而对于陀螺来说，应该是E是不可观测的，但是在图中并不能直观地看出来。
可观性分析
可观性分析代码如下，可以在alignvn函数中实现。
% 去掉哪一个状态变量
remove_idx = 12;
% 备注：状态变量12维
% 可观性分析
Phikk_1 = kf.Phikk_1([1:remove_idx-1,remove_idx+1:12],:);
Phikk_1 = Phikk_1(:,[1:remove_idx-1,remove_idx+1:12]);
Hk = kf.Hk(:,[1:remove_idx-1,remove_idx+1:12]);
Q = [Hk];
Q = [Q;Hk*Phikk_1];
Q = [Q;Hk*Phikk_1^2];
Q = [Q;Hk*Phikk_1^3];
Q = [Q;Hk*Phikk_1^4];
Q = [Q;Hk*Phikk_1^5];
Q = [Q;Hk*Phikk_1^6];
Q = [Q;Hk*Phikk_1^7];
Q = [Q;Hk*Phikk_1^8];
Q = [Q;Hk*Phikk_1^9];
Q = [Q;Hk*Phikk_1^10];
% Q = [Q;Hk*Phikk_1^11];
rank_Q = rank(Q);
fprintf("rank_Q = %d\n",rank_Q);
开始阶段状态向量为12维：失准角（3），速度误差（3），陀螺零偏（3），加表零偏（3），计算得到的全状态可观性矩阵的秩为9，但是不太清楚哪些状态量是可观的，哪些是不可观的。接下来去掉一个状态变量，看秩的变化，结果如下所示：
% 全状态：9
% 去掉1:8
% 去掉2:8
% 去掉3:8
% 去掉4:7
% 去掉5:7
% 去掉6:7
% 去掉7:9
% 去掉8:9
% 去掉9:9
% 去掉10:9
% 去掉11:9
% 去掉12:9
从上面的结果中还是可以看到一些规律的，失准角对应的是8，速度误差对应的是7，陀螺和加表零偏对应的是9。通过严老师书中的分析可知，速度误差直接来源自量测，失准角对应量测的一次导数，其他是二次导数或者三次导数，导数的阶数越高，对应的可观测度越差。和上述的结果基本一致，从另外一个侧面可以反应可观测度。
扫一扫
4862
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
