026慣導衛星組合導航模擬
阿新 • • 發佈：2018-11-09
這裡說明部分函式。
1、卡爾曼濾波器初始化
% 卡爾曼濾波器初始化 function kf = kfinit(Qk, Rk, P0, Phikk_1, Hk, Tauk) [kf.m, kf.n] = size(Hk); kf.Qk = Qk; kf.Rk = Rk; kf.Pk = P0; kf.Xk = zeros(kf.n,1); kf.Phikk_1 = Phikk_1; % 零矩陣 kf.Hk = Hk; % 沒有輸入系統噪聲分配矩陣，預設為單位陣 if nargin < 6 kf.Tauk = eye(kf.n); else kf.Tauk = Tauk; end end
2、慣性感測器資料注入誤差
% 慣性感測器資料注入誤差 function [wm, vm] = imuadderr(wm, vm, eb, web, db, wdb, ts) % IMU新增零偏與遊走誤差 m = size(wm,1); % 獲取行數 sts = sqrt(ts); wm = wm + [ ts*eb(1) + sts*web(1)*randn(m,1), ... ts*eb(2) + sts*web(2)*randn(m,1), ... ts*eb(3) + sts*web(3)*randn(m,1) ]; vm = vm + [ ts*db(1) + sts*wdb(1)*randn(m,1), ... ts*db(2) + sts*wdb(2)*randn(m,1), ... ts*db(3) + sts*wdb(3)*randn(m,1) ]; end
這裡不要忘了給資料的每一行都新增誤差。
3、SINS誤差轉移矩陣
% SINS誤差轉移矩陣 % 參考4.2.5和6.3.3節及earth函式 function Ft = kfft15(eth, Cnb, fb) global g0 tl = eth.tl; secl = 1/eth.cl; f_RMh = 1/eth.RMh; f_RNh = 1/eth.RNh; f_clRNh = 1/eth.clRNh; f_RMh2 = f_RMh*f_RMh; f_RNh2 = f_RNh*f_RNh; vE_clRNh = eth.vn(1)*f_clRNh; vE_RNh2 = eth.vn(1)*f_RNh2; vN_RMh2 = eth.vn(2)*f_RMh2; Mp1 = [ 0, 0, 0; -eth.wnie(3), 0, 0; eth.wnie(2), 0, 0 ]; Mp2 = [ 0, 0, vN_RMh2; 0, 0, -vE_RNh2; vE_clRNh*secl, 0, -vE_RNh2*tl]; Maa = askew(-eth.wnin); Mav = [ 0, -f_RMh, 0; f_RNh, 0, 0; f_RNh*tl, 0, 0 ]; Map = Mp1 + Mp2; Mva = askew(Cnb*fb); Mvv = askew(eth.vn)*Mav - askew(eth.wnien); Mvp = askew(eth.vn) * (Mp1+Map); scl = eth.sl * eth.cl; Mvp(3,1) = Mvp(3,1) - g0 * (5.27094e-3*2*scl+2.32718e-5*4*eth.sl2*scl); Mvp(3,3) = Mvp(3,3) + 3.086e-6; Mpv = [ 0, f_RMh, 0; f_clRNh, 0, 0; 0, 0, 1 ]; Mpp = [ 0, 0, -vN_RMh2; vE_clRNh*tl, 0, -vE_RNh2*secl; 0, 0, 0 ]; O33 = zeros(3); % phi dvn dpos eb db Ft = [ Maa Mav Map -Cnb O33 Mva Mvv Mvp O33 Cnb O33 Mpv Mpp O33 O33 zeros(6,15) ]; end
該部分參考earth函式以及4.2.5和6.3.3節。注意這裡的F是15*15，沒有杆臂和時間誤差。
4、卡爾曼濾波器更新
% 卡爾曼濾波更新
% 參考5.2節
% kf - 卡爾曼濾波結構體陣列
% Zk - 量測向量
% TimeMeasBoth - 更新判斷條件,
% TimeMeasBoth='T' (nargin==1) 僅根據時間更新系統狀態
% TimeMeasBoth='M' 僅根據量測值更新系統狀態
% TimeMeasBoth='B' (nargin==2) 根據時間和量測值更新系統狀態
function kf = kfupdate(kf, Zk, TimeMeasBoth)
% 判斷輸入元素個數
if nargin==1 % 僅有kf輸入,僅根據時間更新系統狀態
TimeMeasBoth = 'T';
elseif nargin==2 % 兩個輸入,根據時間和量測值更新系統狀態
TimeMeasBoth = 'B';
end
% 根據時間進行系統狀態的更新
if TimeMeasBoth=='T' || TimeMeasBoth=='B'
kf.Xkk_1 = kf.Phikk_1*kf.Xk; % 參考(5.2-5)
kf.Pkk_1 = kf.Phikk_1*kf.Pk*kf.Phikk_1' + kf.Tauk*kf.Qk*kf.Tauk';
% 參考(5.2-7)
else % TimeMeasBoth=='M'
kf.Xkk_1 = kf.Xk;
kf.Pkk_1 = kf.Pk;
end
% 根據量測值對系統狀態進行更新
if TimeMeasBoth=='M' || TimeMeasBoth=='B'
kf.PXZkk_1 = kf.Pkk_1*kf.Hk'; % 參考(5.2-13)
kf.PZkk_1 = kf.Hk*kf.PXZkk_1 + kf.Rk; % 結合(5.2-12)(5.2-13)
kf.Kk = kf.PXZkk_1/kf.PZkk_1; % 參考(5.2-28)
kf.Xk = kf.Xkk_1 + kf.Kk*(Zk-kf.Hk*kf.Xkk_1);% 參考(5.2-15)
kf.Pk = kf.Pkk_1 - kf.Kk*kf.PZkk_1*kf.Kk'; % 結合(5.2-12)(5.2-31b)
else % TimeMeasBoth=='T'
kf.Xk = kf.Xkk_1;
kf.Pk = kf.Pkk_1;
end
kf.Pk = (kf.Pk+kf.Pk')/2; % 對稱化
end
5、組合導航模擬主程式
% 慣導與衛星組合導航模擬
clear
clc
glvs;
nn = 2;
ts = 0.1;
nts = nn * ts;
att0 = [0; 0; 30] * arcdeg;
qnb0 = a2qua(att0);
vn0 = [0;0;0];
pos0 = [34*arcdeg; 108*arcdeg; 100];
qnb = qnb0;
vn = vn0;
pos = pos0;
eth = earth(pos, vn);
wm = qmulv(qconj(qnb),eth.wnie) * ts;
vm = qmulv(qconj(qnb),-eth.gn) * ts;
wm = repmat(wm', nn, 1);
vm = repmat(vm', nn, 1);
phi = [0.1; 0.2; 3] * arcmin;
qnb = qaddphi(qnb, phi);
%****至此與慣導模擬相同，以下進行註釋****
eb = [0.01;0.015;0.02] * dph; % 陀螺隨機常值漂移誤差 度/秒
web = [0.001;0.001;0.001] * dpsh; % 角度隨機遊走係數 度/sqrt(秒)
db = [80;90;100] * ug; % 加速度計隨機常值偏值誤差 m/s^2
wdb = [1;1;1] * ugpsHz; % 速度隨機遊走係數
Qk = diag([web; wdb; zeros(9,1)])^2 * nts; % 系統噪聲方差陣 15*15
rk = [[0.1;0.1;0.1];[[10;10]/Re;10]]; % 6*1
Rk = diag(rk)^2; % 6*6
P0 = diag([[0.1;0.1;10]*arcdeg; ...
[1;1;1]; ...
[[10;10]/Re;10]; ...
[0.1;0.1;0.1]*dph; ...
[100;100;100]*ug])^2; % 15*15
Hk = [zeros(6,3),eye(6),zeros(6)]; % 6*15
kf = kfinit(Qk, Rk, P0, zeros(15), Hk); % 卡爾曼濾波器初始化
len = fix(3600/ts); % 模擬時長
avp = zeros(len, 10); % 記錄結果
xkpk = zeros(len, 2*kf.n+1);
kk = 1;
t = 0;
for k=1:nn:len
t = t + nts;
[wm1, vm1] = imuadderr(wm, vm, eb, web, db, wdb, ts);
[qnb, vn, pos, eth] = insupdate(qnb, vn, pos, wm1, vm1, ts);
kf.Phikk_1 = eye(15) + kfft15(eth, q2mat(qnb), sum(vm1,1)'/nts)*nts;
kf = kfupdate(kf);
if mod(t,1)<nts % 整秒時執行下列操作
gps = [vn0; pos0] + rk.*randn(6,1); % GPS速度位置模擬,新增誤差
kf = kfupdate(kf, [vn;pos]-gps, 'M'); % sins與gps差值作為量測值修正系統狀態
vn(3) = vn(3) - kf.Xk(6);
kf.Xk(6) = 0;
end
% 記錄
avp(kk,:) = [qq2phi(qnb,qnb0); vn; pos; t]';
xkpk(kk,:) = [kf.Xk; diag(kf.Pk); t];
kk = kk+1;
% 顯示進度
if mod(t,500)<nts
disp(fix(t));
end
end
avp(kk:end,:) = [];
xkpk(kk:end,:) = [];
tt = avp(:,end);
I1 = ones(size(tt));
% 狀態真值與估計效果對比圖
mysubplot(321, tt, [avp(:,1:2),xkpk(:,1:2)]/arcmin, '\phi_E,\phi_N / \prime');
mysubplot(322, tt, [avp(:,3),xkpk(:,3)]/arcmin, '\phi_U / \prime');
mysubplot(323, tt, [avp(:,4:6),xkpk(:,4:6)], '\deltav ^n / m/s');
mysubplot(324, tt, [deltapos(avp(:,7:9)),[xkpk(:,7),xkpk(:,8).*cos(avp(:,7))]*Re,xkpk(:,9)], '\Deltap / m');
mysubplot(325, tt, [eb(1)*I1,eb(2)*I1,eb(3)*I1,xkpk(:,10:12)]/dph, '\epsilon / \circ/h');
mysubplot(326, tt, [db(1)*I1,db(2)*I1,db(3)*I1,xkpk(:,13:15)]/ug, '\nabla / ug');
% 方差收斂圖
pk = sqrt(xkpk(:,16:end-1));
mysubplot(321, tt, pk(:,1:2)/arcmin, '\phi_E,\phi_N / \prime');
mysubplot(322, tt, pk(:,3)/arcmin, '\phi_U / \prime');
mysubplot(323, tt, pk(:,4:6), '\deltav ^n / m/s');
mysubplot(324, tt, [[pk(:,7),pk(:,8)*cos(avp(1,7))]*Re,pk(:,9)], '\DeltaP / m');
mysubplot(325, tt, pk(:,10:12)/dph, '\epsilon / \circ/h');
mysubplot(326, tt, pk(:,13:15)/ug, '\nabla / ug');