026惯导卫星组合导航仿真

这里说明部分函数。

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 < 6kf.Tauk = eye(kf.n);elsekf.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 g0tl = 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      dbFt = [ Maa    Mav    Map    -Cnb     O33 Mva    Mvv    Mvp     O33     Cnb O33    Mpv    Mpp     O33     O33zeros(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; endkf.Pk = (kf.Pk+kf.Pk')/2;                   % 对称化
end

5、组合导航仿真主程序

% 惯导与卫星组合导航仿真clear
clcglvs;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*15rk = [[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*15kf = 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:lent = 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)<ntsdisp(fix(t));  end
endavp(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');

026惯导卫星组合导航仿真相关推荐

SINS/GNSS组合导航仿真应用详细版(基于PSINS工具箱 )
文章目录轨迹仿真生成轨迹数据绘制仿真数据惯导仿真纯惯导仿真 SINS/GPS组合导航总结 1 2 3 4 5 轨迹仿真生成轨迹数据首先,打开demos\test_trj.m文件,运行仿 ...
PSINS工具箱学习（一）下载安装初始化、SINS-GPS组合导航仿真、习惯约定与常用变量符号、数据导入转换、绘图显示
文章目录一.前言二.相关资源三.下载安装初始化 1.下载PSINSyymmdd.rar工具箱文件 2.解压文件 3.初始化 4.启动工具箱导览四.习惯约定与常用变量符号 1.PSINS全局变量 ...
【滤波跟踪】基于联邦滤波算法实现惯性+GPS+地磁组合导航仿真含Matlab源码
1 简介设计INS/GPS组合导航系统时,考虑到观测量GPS位置和速度是正相关的,可通过降低单个滤波器的维度形成两个局部滤波器,主滤波器融合局部滤波器的状态估计,得到整个组合导航系统的误差状态估 ...
一种惯导/北斗组合导航的半实物仿真测试方法ETest
设备组成 Etest_CPS系统主要由硬件部分与软件部分组成.硬件部分由PCI机箱.PCI控制器以及各种PCI接口板卡组成.软件部分由测试设计软件模块.测试执行服务软件模块.测试执行客户端软件模块.设 ...
PSINS开源代码初体验——航迹仿真与组合导航
文章目录航迹仿真组合导航仿真 PSINS是西北工业大学严恭敏教授开发的高精度捷联惯性系统及其组合导航系统开源软件(matlab, C++),软件及其他参考资料下载可以参见www.psins.org ...
INS/GNSS组合导航（七）-SINS的微分方程的推导
(三)中对SINS的机械编排进行了初步可行性的介绍,并未对机械编排进行原理性介绍.那么在详细介绍机械编排之前,需要先对SINS的微分方程进行详细的推导. 无论是机械编排,还是后面误差方程的建立,SIN ...
用卡尔曼滤波处理工程数据的方法与思考with基于GPS与INS组合导航的滤波模型仿真
Say Something: 我猜能看到这个小文章的小伙伴估计已经为了学卡尔曼滤波费劲了头脑,查遍了资料.而且我推测这里的大多数人在之前的学习过程中总是发现那些资料里总是用一些理想的模型举例子,而且针 ...
捷联惯导基础知识解析之四（粗/精对准和GPS/IMU和GPS/里程计组合导航）
初始对准(粗.精对准)/组合导航一.捷联惯导粗对准目的:寻找.确定参考导航坐标系:结果表现形式:得到姿态矩阵(进而可以求出欧拉角.四元数等) 前提:在导航坐标系(比如:东北天)下的重力矢量.地球旋 ...
捷联惯导算法与组合导航原理学习——四元数和姿态阵转换（二）
四元数和姿态阵转换学习资料参考: [1] 严恭敏,翁浚. 捷联惯导算法与组合导航原理[M]. 西安: 西北工业大学出版社, 2019.8. Quaternion.h #pragma once #in ...

026惯导卫星组合导航仿真

1、卡尔曼滤波器初始化

2、惯性传感器数据注入误差

3、SINS误差转移矩阵

4、卡尔曼滤波器更新

5、组合导航仿真主程序

026惯导卫星组合导航仿真相关推荐

最新文章

热门文章