版权声明：本文系原创文章，没有版权，随意转载，不用告知！ https://blog.csdn.net/Pro2015/article/details/82984472
这里说明部分函数。
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');