test_SINS_GPS_153源码

poserrset

function poserr = poserrset(dpos0, dlon, dhgt)
% position errors dpos=[dlat;dlon;dhgt] setting.
%
% Prototype: poserr = poserrset(dpos0)
% Input: dpos0=[dlat; dlon; dhgt], NOTE: dlat, dlon and dhgt are all in m.
% Output: poserr=[dpos0(1)/Re; dpos0(2)/Re; dpos0(3)], so poserr(1)=dlat,
%              poserr(2)=dlon are in rad and poserr(3)=dhgt is in m.
%
% See also  avperrset, vperrset, posset.% Copyright(c) 2009-2014, by Gongmin Yan, All rights reserved.
% Northwestern Polytechnical University, Xi An, P.R.China
% 09/03/2014
global glvif nargin==3, dpos0=[dpos0;dlon; dhgt]; end  % dpos = poserrset(dlat, dlon, dhgt)if nargin==2, dpos0=[dpos0;dpos0; dlon]; end  % dpos = poserrset(dlat_dlon, dhgt)dpos0 = rep3(dpos0);poserr = [dpos0(1:2)/glv.Re; dpos0(3)];

kfinit

function kf = kfinit(ins, varargin)
% Kalman filter initializes for structure array 'kf', this precedure
% usually includs the setting of structure fields: Qt, Rk, Pxk, Hk.
%
% Prototype: kf = kfinit(ins, varargin)
% Inputs: ins - SINS structure array, if not struct then nts=ins;
%         varargin - if any other parameters
% Output: kf - Kalman filter structure array
%
% See also  kfinit0, kfsetting, kffk, kfkk, kfupdate, kffeedback, psinstypedef.% Copyright(c) 2009-2014, by Gongmin Yan, All rights reserved.
% Northwestern Polytechnical University, Xi An, P.R.China
% 09/10/2013
global glv psinsdef
[Re,deg,dph,ug,mg] = ... % just for shortsetvals(glv.Re,glv.deg,glv.dph,glv.ug,glv.mg);
o33 = zeros(3); I33 = eye(3);
kf = [];
if isstruct(ins),    nts = ins.nts;
else                 nts = ins;
end
switch(psinsdef.kfinit)case psinsdef.kfinit153,psinsdef.kffk = 15;  psinsdef.kfhk = 153;  psinsdef.kfplot = 15;[davp, imuerr, rk] = setvals(varargin);kf.Qt = diag([imuerr.web; imuerr.wdb; zeros(9,1)])^2;kf.Rk = diag(rk)^2;kf.Pxk = diag([davp; imuerr.eb; imuerr.db]*1.0)^2;kf.Hk = kfhk(0);case psinsdef.kfinit156,psinsdef.kffk = 15;  psinsdef.kfhk = 156;  psinsdef.kfplot = 15;[davp, imuerr, rk] = setvals(varargin);kf.Qt = diag([imuerr.web; imuerr.wdb; zeros(9,1)])^2;kf.Rk = diag(rk)^2;kf.Pxk = diag([davp; imuerr.eb; imuerr.db]*1.0)^2;kf.Hk = kfhk(0);case psinsdef.kfinit183,psinsdef.kffk = 18;  psinsdef.kfhk = 183;  psinsdef.kfplot = 18;[davp, imuerr, lever, r0] = setvals(varargin);kf.Qt = diag([imuerr.web; imuerr.wdb; zeros(9+3,1)])^2;kf.Rk = diag(r0)^2;kf.Pxk = diag([davp; imuerr.eb; imuerr.db; lever]*1.0)^2;kf.Hk = zeros(3,18);case psinsdef.kfinit186,psinsdef.kffk = 18;  psinsdef.kfhk = 186;  psinsdef.kfplot = 18;[davp, imuerr, lever, r0] = setvals(varargin);kf.Qt = diag([imuerr.web; imuerr.wdb; zeros(3,1); imuerr.sqg; imuerr.sqa; zeros(3,1)])^2;kf.Rk = diag(r0)^2;kf.Pxk = diag([davp; imuerr.eb; imuerr.db; lever]*1.0)^2;kf.Hk = zeros(6,18);case psinsdef.kfinit193psinsdef.kffk = 19;  psinsdef.kfhk = 193;  psinsdef.kfplot = 19;[davp, imuerr, lever, dT, r0] = setvals(varargin);kf.Qt = diag([imuerr.web; imuerr.wdb; [1/Re;1/Re;1]*glv.mpsh; ...[1;1;1]*0*glv.dphpsh; [1;1;1]*0*glv.ugpsh; [1;1;1]*0.*glv.mpsh; 0])^2;kf.Rk = diag(r0)^2;kf.Pxk = diag([davp; imuerr.eb; imuerr.db; lever; dT]*1.0)^2;kf.Hk = zeros(3,19);case psinsdef.kfinit196psinsdef.kffk = 19;  psinsdef.kfhk = 196;  psinsdef.kfplot = 19;[davp, imuerr, lever, dT, r0] = setvals(varargin);kf.Qt = diag([imuerr.web; imuerr.wdb; [1/Re;1/Re;1]*0*glv.mpsh; ...[1;1;1]*0*glv.dphpsh; [1;1;1]*0*glv.ugpsh; [1;1;1]*0*glv.mpsh; 0])^2;kf.Rk = diag(r0)^2;kf.Pxk = diag([davp; imuerr.eb; imuerr.db; lever; dT]*1.0)^2;kf.Hk = zeros(6,19);case psinsdef.kfinit331,psinsdef.kffk = 33;  psinsdef.kfhk = 331;  psinsdef.kfplot = 33;[davp, imuerr, r0] = setvals(varargin);kf.Qt = diag([imuerr.web; imuerr.wdb; zeros(9+15+3,1)])^2;kf.Rk = diag(r0)^2;kf.Pxk = diag([davp; imuerr.eb; imuerr.db; imuerr.dKga; imuerr.KA2])^2;kf.Hk = kfhk(ins);kf.xtau(1:psinsdef.kffk,1) = 0;case psinsdef.kfinit346,psinsdef.kffk = 34;  psinsdef.kfhk = 346;  psinsdef.kfplot = 34;[davp, imuerr, lever, dT, r0] = setvals(varargin);kf.Qt = diag([imuerr.web; imuerr.wdb; zeros(9+3+1+15,1)])^2;kf.Rk = diag(r0)^2;kf.Pxk = diag([davp; imuerr.eb; imuerr.db; lever; dT; imuerr.dKga])^2;kf.Hk = kfhk(ins);kf.xtau(1:psinsdef.kffk,1) = 0;case psinsdef.kfinit376,psinsdef.kffk = 37;  psinsdef.kfhk = 376;  psinsdef.kfplot = 37;[davp, imuerr, lever, dT, r0] = setvals(varargin);kf.Qt = diag([imuerr.web; imuerr.wdb; zeros(9+3+1+15+3,1)])^2;kf.Rk = diag(r0)^2;kf.Pxk = diag([davp; imuerr.eb; imuerr.db; lever; dT; imuerr.dKga; davp(4:6)]*10)^2;kf.Hk = kfhk(ins);kf.xtau(1:psinsdef.kffk,1) = 0;otherwise,kf = feval(psinsdef.typestr, psinsdef.kfinittag, [{ins},varargin]);
end
kf = kfinit0(kf, nts);

gabias

function bias = gabias(gbias, abias)
% Gyro & Acc constant bias setting.
%
% Prototype: Gyro & Acc constant bias setting
% Inputs: gbias - gyro bias, in deg/h
%         abias - acc bias, in ug
% Output: bias - = [gbias; abias]
%
% See also  imuerrset, avpset, insupdate.% Copyright(c) 2009-2021, by Gongmin Yan, All rights reserved.
% Northwestern Polytechnical University, Xi An, P.R.China
% 06/02/2021
global glvbias = [rep3(gbias)*glv.dph; rep3(abias)*glv.ug];

kfupdate

function kf = kfupdate(kf, yk, TimeMeasBoth)
% Discrete-time Kalman filter.
%
% Prototype: kf = kfupdate(kf, yk, TimeMeasBoth)
% Inputs: kf - Kalman filter structure array
%         yk - measurement vector
%         TimeMeasBoth - described as follows,
%            TimeMeasBoth='T' (or nargin==1) for time updating only,
%            TimeMeasBoth='M' for measurement updating only,
%            TimeMeasBoth='B' (or nargin==2) for both time and
%                             measurement updating.
% Output: kf - Kalman filter structure array after time/meas updating
% Notes: (1) the Kalman filter stochastic models is
%      xk = Phikk_1*xk_1 + wk_1
%      yk = Hk*xk + vk
%    where E[wk]=0, E[vk]=0, E[wk*wk']=Qk, E[vk*vk']=Rk, E[wk*vk']=0
%    (2) If kf.adaptive=1, then use Sage-Husa adaptive method (but only for
%    measurement noise 'Rk'). The 'Rk' adaptive formula is:
%      Rk = b*Rk_1 + (1-b)*(rk*rk'-Hk*Pxkk_1*Hk')
%    where minimum constrain 'Rmin' and maximum constrain 'Rmax' are
%    considered to avoid divergence.
%    (3) If kf.fading>1, then use fading memory filtering method.
%    (4) Using Pmax&Pmin to constrain Pxk, such that Pmin<=diag(Pxk)<=Pmax.
%
% See also  kfinit, kfupdatesq, kffk, kfhk, kfc2d, kffeedback, kfplot, RLS, ekf, ukf.% Copyright(c) 2009-2015, by Gongmin Yan, All rights reserved.
% Northwestern Polytechnical University, Xi An, P.R.China
% 08/12/2012, 29/08/2013, 16/04/2015, 01/06/2017, 11/03/2018if nargin==1;TimeMeasBoth = 'T';elseif nargin==2TimeMeasBoth = 'B';endif TimeMeasBoth=='T'            % Time Updatingkf.xk = kf.Phikk_1*kf.xk;    kf.Pxk = kf.Phikk_1*kf.Pxk*kf.Phikk_1' + kf.Gammak*kf.Qk*kf.Gammak';elseif TimeMeasBoth=='M'        % Meas Updatingkf.xkk_1 = kf.xk;    kf.Pxkk_1 = kf.Pxk; elseif TimeMeasBoth=='B'    % Time & Meas Updatingkf.xkk_1 = kf.Phikk_1*kf.xk;    kf.Pxkk_1 = kf.Phikk_1*kf.Pxk*kf.Phikk_1' + kf.Gammak*kf.Qk*kf.Gammak';elseerror('TimeMeasBoth input error!');endkf.Pxykk_1 = kf.Pxkk_1*kf.Hk';kf.Py0 = kf.Hk*kf.Pxykk_1;kf.ykk_1 = kf.Hk*kf.xkk_1;kf.rk = yk-kf.ykk_1;if kf.adaptive==1  % for adaptive KF, make sure Rk is diag 24/04/2015for k=1:kf.mif yk(k)>1e10, continue; end  % 16/12/2019ry = kf.rk(k)^2-kf.Py0(k,k);if ry<kf.Rmin(k,k), ry = kf.Rmin(k,k); endif ry>kf.Rmax(k,k),     kf.Rk(k,k) = kf.Rmax(k,k);else                  kf.Rk(k,k) = (1-kf.beta)*kf.Rk(k,k) + kf.beta*ry;endendkf.beta = kf.beta/(kf.beta+kf.b);endkf.Pykk_1 = kf.Py0 + kf.Rk;kf.Kk = kf.Pxykk_1*invbc(kf.Pykk_1); % kf.Kk = kf.Pxykk_1*kf.Pykk_1^-1;kf.xk = kf.xkk_1 + kf.Kk*kf.rk;kf.Pxk = kf.Pxkk_1 - kf.Kk*kf.Pykk_1*kf.Kk';kf.Pxk = (kf.Pxk+kf.Pxk')*(kf.fading/2); % symmetrization & forgetting factor 'fading'
%         if kf.adaptive==1
%             krrk = kf.Kk*kf.rk*kf.rk'*kf.Kk';
%             for k=3:3
%                 krrki = krrk(k,k) + kf.Pxk(k,k) - kf.Pxkk_1(k,k);
%                 if krrki>kf.Qmax(k,k),     kf.Qk(k,k) = kf.Qmax(k,k);
%                 elseif krrki<kf.Qmin(k,k), kf.Qk(k,k) = (1-kf.beta)*kf.Qk(k,k) + kf.beta*kf.Qmin(k,k);
%                 else                  kf.Qk(k,k) = (1-kf.beta)*kf.Qk(k,k) + kf.beta*krrki;
%                 end
%             end
%         endif kf.xconstrain==1  % 16/3/2018for k=1:kf.nif kf.xk(k)<kf.xmin(k)kf.xk(k)=kf.xmin(k);elseif kf.xk(k)>kf.xmax(k)kf.xk(k)=kf.xmax(k);endendendif kf.pconstrain==1  % 1/6/2017for k=1:kf.nif kf.Pxk(k,k)<kf.Pmin(k)kf.Pxk(k,k)=kf.Pmin(k);elseif kf.Pxk(k,k)>kf.Pmax(k)ratio = sqrt(kf.Pmax(k)/kf.Pxk(k,k));kf.Pxk(:,k) = kf.Pxk(:,k)*ratio;  kf.Pxk(k,:) = kf.Pxk(k,:)*ratio;endendendend

流程

1.rk = poserrset([1;1;3])设置位置测量误差
2.kf = kfinit(ins, davp0, imuerr, rk)卡尔曼滤波器初始化，主要初始化一些方阵，如系统状态噪声方差阵Q、量测噪声方差阵R、误差协方差阵P以及观测矩阵H
3.kf.Pmin = [avperrset(0.01,1e-4,0.1); gabias(1e-3, [1,10])].^2设置方差阵最小值，包括位置误差和陀螺、加速度计的常值零偏
4.kf.pconstrain=1表示对方差阵上下限进行约束
5.len = length(imu)求IMU原始测量值的行数
6.[avp, xkpk] = prealloc(fix(len/nn), 10, 2*kf.n+1)循环使用变量前，先为变量分配内存，其中第一个参数为行数，第二个参数、第三个参数分别为第一个变量、第二个变量的列数
7.timebar(nn, len, '15-state SINS/GPS Simulation.')程序进度条
8.ki = 1量测更新计数
9.for k=1:nn:len-nn+1控制惯导更新循环
10.k1 = k+nn-1控制多子样数据输入
11.wvm = imu(k:k1,1:6); t = imu(k1,end)角增量、速度增量以及时间赋值
12.ins = insupdate(ins, wvm)惯导更新
13.kf.Phikk_1 = kffk(ins)计算离散时间转移矩阵
14.kf = kfupdate(kf)卡尔曼滤波器的时间更新
15.if mod(t,1)==0每整秒的时候，使用GNSS测量值进行组合导航
16.posGPS = trj.avp(k1,7:9)' + davp0(7:9).*randn(3,1)用白噪声来仿真GNSS位置
17. kf = kfupdate(kf, ins.pos-posGPS, 'M')卡尔曼滤波器的量测更新
18. [kf, ins] = kffeedback(kf, ins, 1, 'avp')卡尔曼滤波状态估计反馈给捷联惯导系统
19. avp(ki,:) = [ins.avp', t]存储姿态、速度和位置信息
20. xkpk(ki,:) = [kf.xk; diag(kf.Pxk); t]'存储反馈后剩余状态、方差阵以及时间
21. ki = ki+1更新组合导航量测更新计数
22. avp(ki:end,:) = []; xkpk(ki:end,:) = [] 清除未利用的分配空间

PSINS工具箱15状态组合导航仿真程序(test_SINS_GPS_153)浅析-卡尔曼滤波设置+导航解算相关推荐

PSINS工具箱15状态组合导航仿真程序(test_SINS_GPS_153)浅析-初始化设置
文章目录 test_SINS_GPS_153源码主函数 psinstypedef imuerrset imuadderr avperrset avpadderr insinit 流程 test_SI ...
Psins代码解析之全局变量轨迹仿真（test_SINS_trj.m）惯性解算（test_SINS.m）
旋转椭球体的4个基本参数:长半轴.扁率(椭圆度).地心引力常数.自转角速率: 以上内容来自:<车载定位定向系统关键技术研究>付强文旋转椭球体: 地球自转角速度: 地球重力加速度为: 子午 ...
iOS设置导航栏和状态栏
文章目录 iOS 15 之后导航栏背景色的设置 1.状态栏设置 1.1.没有导航栏 1.2.有导航栏 2.导航栏背景和字体颜色 2.1.十六进制颜色转RGB 2.2.生成纯色图片 3.导航栏的另外一种 ...
固定导航栏android,Android 状态栏和导航栏的真终极解决方案
去年我写过一篇文章,透明状态栏和导航栏的终极解决方案,并在 Github 上开源了代码,https://github.com/Zackratos/UltimateBar,其实在那之后,我一直对这个项目 ...
PSINS工具箱学习（一）下载安装初始化、SINS-GPS组合导航仿真、习惯约定与常用变量符号、数据导入转换、绘图显示
文章目录一.前言二.相关资源三.下载安装初始化 1.下载PSINSyymmdd.rar工具箱文件 2.解压文件 3.初始化 4.启动工具箱导览四.习惯约定与常用变量符号 1.PSINS全局变量 ...
AOJ0525 Osenbei【DFS+状态组合】
おせんべい問題 IOI製菓では,創業以来の伝統の製法で煎餅(せんべい)を焼いている．この伝統の製法は,炭火で一定時間表側を焼き,表側が焼けると裏返して,炭火で一定時間裏側を焼くというものである．この ...
PSINS中19维组合导航模块sinsgps详解(滤波部分)
PSINS中19维组合导航模块sinsgps详解滤波部分滤波部分 for k=1:nn:len-nn+1k1 = k+nn-1; wvm = imu(k:k1,1:6); t = imu(k1,e ...
CSS3——对齐组合选择符伪类伪元素导航栏下拉菜单
水平&垂直对齐元素居中对齐 .center {margin: auto;width: 50%;border: 3px solid green;padding: 10px; } 文本居中对齐 ...
excel工具箱_Excel工具箱15.54安装教程
插件下载 [名称]:office插件-excel工具箱 [版本]:15.54 [大小]:3.87 MB [语言]:简体中文 [安装环境]:Win7/Win8/Win10 [支持版本]:Excel200 ...

PSINS工具箱15状态组合导航仿真程序(test_SINS_GPS_153)浅析-卡尔曼滤波设置+导航解算

文章目录