捷联惯导更新算法

首先通过AVP参数使用insinit函数初始化获得导航结构体ins；再调用insupdate函数进行惯导更新。

`insinit`函数初始化

ins结构体初始化的成员较多，在insinit中显示如下：

function ins = insinit(avp0, ts, var1, var2)
% SINS structure array initialization.
%
% Prototype: ins = insinit(avp0, ts, var1, var2)
% Initialization usages(maybe one of the following methods):
%       ins = insinit(avp0, ts);
%       ins = insinit(avp0, ts, avperr);
%       ins = insinit(qnb0, vn0, pos0, ts);
% Inputs: avp0 - initial avp0 = [att0; vn0; pos0]
%         ts - SIMU sampling interval
%         avperr - avp error setting
% Output: ins - SINS structure array
%
% See also  insupdate, avpset, kfinit.% Copyright(c) 2009-2014, by Gongmin Yan, All rights reserved.
% Northwestern Polytechnical University, Xi An, P.R.China
% 22/03/2008, 12/01/2013, 18/03/2014
global glvavp0 = avp0(:);if length(avp0)==1, avp0=zeros(9,1); end  % ins = insinit(0, ts);if length(avp0)==4, avp0=[0;0;avp0(1); 0;0;0; avp0(2:4)]; end  % ins = insinit([yaw;pos], ts);if length(avp0)==6, avp0=[avp0(1:3); 0;0;0; avp0(4:6)]; end  % ins = insinit([att;pos], ts);if nargin==2      % ins = insinit(avp0, ts);[qnb0, vn0, pos0] = setvals(a2qua(avp0(1:3)), avp0(4:6), avp0(7:9));elseif nargin==3  % ins = insinit(avp0, ts, avperr);avperr = var1;avp0 = avpadderr(avp0, avperr);[qnb0, vn0, pos0] = setvals(a2qua(avp0(1:3)), avp0(4:6), avp0(7:9));elseif nargin==4  % ins = insinit(qnb0, vn0, pos0, ts);[qnb0, vn0, pos0, ts] = setvals(avp0, ts, var1, var2);end        ins = [];ins.ts = ts; ins.nts = 2*ts;[ins.qnb, ins.vn, ins.pos] = setvals(qnb0, vn0, pos0); ins.vn0 = vn0; ins.pos0 = pos0;[ins.qnb, ins.att, ins.Cnb] = attsyn(ins.qnb);  ins.Cnb0 = ins.Cnb;ins.avp  = [ins.att; ins.vn; ins.pos];ins.eth = ethinit(ins.pos, ins.vn);% 'wib,web,fn,an,Mpv,MpvCnb,Mpvvn,CW' may be very useful outside SINS,% so we calucate and save them.ins.wib = ins.Cnb'*ins.eth.wnin;ins.fn = -ins.eth.gn;  ins.fb = ins.Cnb'*ins.fn;[ins.wnb, ins.web, ins.an] = setvals(zeros(3,1));ins.Mpv = [0, 1/ins.eth.RMh, 0; 1/ins.eth.clRNh, 0, 0; 0, 0, 1];ins.MpvCnb = ins.Mpv*ins.Cnb;  ins.Mpvvn = ins.Mpv*ins.vn; [ins.Kg, ins.Ka] = setvals(eye(3)); % calibration parameters[ins.eb, ins.db] = setvals(zeros(3,1));[ins.tauG, ins.tauA] = setvals(inf(3,1)); % gyro & acc correlation timeins.lever = zeros(3,1); ins = inslever(ins); % lever armins.tDelay = 0; % time delayins.openloop = 0;glv.wm_1 = zeros(3,1)';  glv.vm_1 = zeros(3,1)';  % for 'single sample+previous sample' coning algorithmins.an0 = zeros(3,1);  ins.anbar = ins.an0;

`insupdate`函数进行惯导更新

insupdate函数先进行不可交换误差补偿->再进行传感器标定->再计算地球相关参数；不可交换误差在前面博客已讲过(圆锥和划桨误差)；传感器的标定主要就是，不正交安装误差，如果在卡尔曼滤波中，若能估计出来陀螺和加速度计零偏，能估计出来，就扣减一下，进行标定。

function ins = insupdate(ins, imu)
% SINS Updating Alogrithm including attitude, velocity and position
% updating.
%
% Prototype: ins = insupdate(ins, imu)
% Inputs: ins - SINS structure array created by function 'insinit'
%         imu - gyro & acc incremental sample(s)
% Output: ins - SINS structure array after updating
%
% See also  insinit, cnscl, earth, trjsimu, imuadderr, avpadderr, q2att,
%           inslever, alignvn, aligni0, etm, kffk, kfupdate, insplot.% Copyright(c) 2009-2014, by Gongmin Yan, All rights reserved.
% Northwestern Polytechnical University, Xi An, P.R.China
% 22/03/2008, 12/01/2013, 18/03/2014, 09/09/2014nn = size(imu,1);nts = nn*ins.ts;  nts2 = nts/2;  ins.nts = nts;[phim, dvbm] = cnscl(imu,0);    % coning & sculling compensation
%     [phim, dvbm] = cnscl0(imu);    % coning & sculling compensationphim = ins.Kg*phim-ins.eb*nts; dvbm = ins.Ka*dvbm-ins.db*nts;  % calibration%% earth & angular rate updating vn01 = ins.vn+ins.an*nts2; pos01 = ins.pos+ins.Mpv*vn01*nts2;  % extrapolation at t1/2if ins.openloop==0, ins.eth = ethupdate(ins.eth, pos01, vn01);elseif ins.openloop==1, ins.eth = ethupdate(ins.eth, ins.pos0, ins.vn0); endins.wib = phim/nts; ins.fb = dvbm/nts;  % same as trjsimuins.web = ins.wib - ins.Cnb'*ins.eth.wnie;
%     ins.wnb = ins.wib - ins.Cnb'*ins.eth.wnin;ins.wnb = ins.wib - (ins.Cnb*rv2m(phim/2))'*ins.eth.wnin;  % 2014-11-30%% (1)velocity updatingins.fn = qmulv(ins.qnb, ins.fb);
%     ins.an = qmulv(rv2q(-ins.eth.wnin*nts2),ins.fn) + ins.eth.gcc;ins.an = rotv(-ins.eth.wnin*nts2, ins.fn) + ins.eth.gcc;  ins.anbar = 0.9*ins.anbar + 0.1*ins.an;vn1 = ins.vn + ins.an*nts;%% (2)position updating
%     ins.Mpv = [0, 1/ins.eth.RMh, 0; 1/ins.eth.clRNh, 0, 0; 0, 0, 1];ins.Mpv(4)=1/ins.eth.RMh; ins.Mpv(2)=1/ins.eth.clRNh;
%     ins.Mpvvn = ins.Mpv*((ins.vn+vn1)/2+(ins.an-ins.an0)*nts^2/3);  % 2014-11-30ins.Mpvvn = ins.Mpv*(ins.vn+vn1)/2;ins.pos = ins.pos + ins.Mpvvn*nts;  ins.vn = vn1;ins.an0 = ins.an;%% (3)attitude updatingins.Cnb0 = ins.Cnb;
%     ins.qnb = qupdt(ins.qnb, ins.wnb*nts);  % lower accuracy than next lineins.qnb = qupdt2(ins.qnb, phim, ins.eth.wnin*nts);[ins.qnb, ins.att, ins.Cnb] = attsyn(ins.qnb);ins.avp = [ins.att; ins.vn; ins.pos];

计算地球相关参数调用ethupdate函数，主要是根据位置pos和速度vn信息计算比力方程中的有害加速度相关项。

function eth = ethupdate(eth, pos, vn)
% Update the Earth related parameters, much faster than 'earth'.
%
% Prototype: eth = ethupdate(eth, pos, vn)
% Inputs: eth - input earth structure array
%         pos - geographic position [lat;lon;hgt]
%         vn - velocity
% Outputs: eth - parameter structure array
%
% See also  ethinit, earth.% Copyright(c) 2009-2014, by Gongmin Yan, All rights reserved.
% Northwestern Polytechnical University, Xi An, P.R.China
% 23/05/2014if nargin==2,  vn = [0; 0; 0];  endeth.pos = pos;  eth.vn = vn;eth.sl = sin(pos(1));  eth.cl = cos(pos(1));  eth.tl = eth.sl/eth.cl; eth.sl2 = eth.sl*eth.sl;  sl4 = eth.sl2*eth.sl2;sq = 1-eth.e2*eth.sl2;  RN = eth.Re/sqrt(sq); eth.RNh = RN+pos(3);  eth.clRNh = eth.cl*eth.RNh;eth.RMh = RN*(1-eth.e2)/sq+pos(3);
%     eth.wnie = [0; eth.wie*eth.cl; eth.wie*eth.sl];eth.wnie(2) = eth.wie*eth.cl; eth.wnie(3) = eth.wie*eth.sl;
%     eth.wnen = [-vn(2)/eth.RMh; vn(1)/eth.RNh; vn(1)/eth.RNh*eth.tl];eth.wnen(1) = -vn(2)/eth.RMh; eth.wnen(2) = vn(1)/eth.RNh; eth.wnen(3) = eth.wnen(2)*eth.tl;
%     eth.wnin = eth.wnie + eth.wnen;eth.wnin(1) = eth.wnie(1) + eth.wnen(1); eth.wnin(2) = eth.wnie(2) + eth.wnen(2); eth.wnin(3) = eth.wnie(3) + eth.wnen(3);
%     eth.wnien = eth.wnie + eth.wnin;eth.wnien(1) = eth.wnie(1) + eth.wnin(1); eth.wnien(2) = eth.wnie(2) + eth.wnin(2); eth.wnien(3) = eth.wnie(3) + eth.wnin(3);
%     eth.gn = [0;0;-eth.g];eth.g = eth.g0*(1+5.27094e-3*eth.sl2+2.32718e-5*sl4)-3.086e-6*pos(3); % grs80eth.gn(3) = -eth.g;
%     eth.gcc = eth.gn - cros(eth.wnien,vn); % Gravitational/Coriolis/Centripetal acceleration
%     eth.gcc =  [ eth.wnien(3)*vn(2)-eth.wnien(2)*vn(3);  % faster than previous line
%                  eth.wnien(1)*vn(3)-eth.wnien(3)*vn(1);
%                  eth.wnien(2)*vn(1)-eth.wnien(1)*vn(2)+eth.gn(3) ];eth.gcc(1) = eth.wnien(3)*vn(2)-eth.wnien(2)*vn(3);eth.gcc(2) = eth.wnien(1)*vn(3)-eth.wnien(3)*vn(1);eth.gcc(3) = eth.wnien(2)*vn(1)-eth.wnien(1)*vn(2)+eth.gn(3);

最后，insupdate先后依次完成速度更新、位置更新和姿态更新。

PSINS捷联惯导更新算法相关推荐

028捷联惯导更新算法备忘
1.姿态更新对于nnn及bbb系,假定有如下四元数转换关系: 那么: rn(k+1)=Cb(k+1)n(k+1)rb(k+1)=Cn(k)n(k+1)Cn(k)b(k)Cb(k)n(k)Cb ...
组合导航算法（一）之捷联惯导更新及组合模式
捷联惯导基本算法惯性导航技术于20世纪50年代最初开始投入使用,可分为物理平台与模拟平台.物理平台就是平台式惯性导航系统(PINS).模拟平台又称捷联式惯性导航系统(SINS),它以计算机为平台,随 ...
基于matlab的捷联惯导算法设计及仿真,基于 Matlab 的捷联惯导算法设计及仿真1doc.doc...
基于 Matlab 的捷联惯导算法设计及仿真1doc 基于 Matlab 的捷联惯导算法设计及仿真1 严恭敏西北工业大学航海学院,西安 (710072) E-mail:yangongmin@163. ...
捷联惯导基础知识解析之五（低成本姿态航向参考系统）
陀螺仪精度0.1°/s:加速度计精度5mg:主要指零偏重复性! 一.简化的惯导算法 1.姿态更新陀螺仪输出直接进行积分,得到角增量: 2.速度更新: 在导航系下,完整的速度微分方程,如下: 其中,在 ...
基于四元素法的捷联惯导姿态更新算法
摘要本文主要介绍了机载捷联惯导系统常用的姿态更新算法--四元素法,并重点介绍了利用四元素法进行姿态更新的一般过程. 关键词:四元素法,连贯导,姿态 1 引言 ...
捷联惯导系统学习4.1(惯导数值更新算法)
1 常用坐标系的定义 (1)地心惯性坐标系(i 系,inertial frame) 用oixiyizio_ix_iy_iz_ioixiyizi表示,原点以地球为中心, 原点oio_ioi在地 ...
捷联惯导算法（二）位置更新算法的理解
前言文中算法公式摘自<捷联惯导算法与组合导航原理>(严恭敏.翁浚编著).<惯性导航>(秦永元编著),其他理解仅代表个人观点.本文是对位置更新算法,按照自己学习的思路整理得 ...
捷联惯导算法--体会与心得
本文转自:http://www.amobbs.com/thread-5492189-1-1.html,收藏学习! 1.四个概念:"地理"坐标系."机体"坐标系. ...
【算法学习笔记001】捷联惯导算法心得
1.四个概念:"地理"坐标系."机体"坐标系.他们之间换算公式.换算公式用的系数. 地理坐标系:东.北.天,以下简称地理.在这个坐标系里有重力永远是(0,0,1 ...
捷联惯导总结--初始对准，位置标定，INS姿态更新，GPS/INS组合
惯导及组合导航回顾 2018.09.16 今天和17系的同学一起把惯导的流程捋了一遍,为了加深自己的记忆,这里在前面把心得大致列出来. 我们这里只考虑捷联式惯导及松组合首先拿到惯性传感器(加速度计 ...

PSINS捷联惯导更新算法

文章目录

捷联惯导更新算法

`insinit`函数初始化

`insupdate`函数进行惯导更新

PSINS捷联惯导更新算法相关推荐

最新文章

热门文章

PSINS捷联惯导更新算法

文章目录

捷联惯导更新算法

insinit函数初始化

insupdate函数进行惯导更新

PSINS捷联惯导更新算法相关推荐

最新文章

热门文章

`insinit`函数初始化

`insupdate`函数进行惯导更新