021静态惯性器件捷联惯导仿真(3)

019静态惯性器件捷联惯导仿真(1)

020静态惯性器件捷联惯导仿真(2)


主函数

%捷联惯导仿真主程序
%东北天坐标系,东X北Y天Z

clear
clc

glvs;           % 加载全局变量

% 子样数和采样时间
nn = 2;         % 子样数,下面也将采用二子样的圆锥误差补偿算法
ts = 0.1;       % 单个采样间隔的采样时间长度
nts = nn * ts;  % 采样周期,即在nts时间内进行两次采样,每次采样0.1秒

%姿态、速度和位置初始化
att = [0; 0; 30] *arcdeg;            % 初始化姿态角,分别为俯仰角θ、横滚角γ、航向角ψ
vn = [0; 0; 0];                      % 初始化速度
pos = [34*arcdeg; 108*arcdeg; 100];  % 初始化位置,纬度34度,经度108度,高度100m
qnb = a2qua(att);                    % 姿态角转换为四元数 b to n

%仿真静态IMU数据
eth = earth(pos, vn);                  % 地球导航参数计算
wm = qmulv(qconj(qnb), eth.wnie) *ts;  % 地球自转角速度由导航系换算到机体系,作为机体角速度,并乘时间得角度增量
vm = qmulv(qconj(qnb), -eth.gn) *ts;   % 比力由导航系换算到机体系,作为机体比力,并乘时间得速度增量
wm = repmat(wm', nn, 1);               % 转置角速度矢量,并复制得到nn行'1'列
vm = repmat(vm', nn, 1);               % 转置速度矢量,并复制得到nn行'1'列

%仿真时长
phi = [0.1; 0.2; 3] *arcmin;   % 失准角误差
qnb = qaddphi(qnb, phi);       % 添加失准角误差的由机体系到导航系的四元数
len = fix(3600/ts);            % 仿真时长一小时,得到fix(3600/ts)个子样

%记录导航结果 [att, vn, pos, t]
avp = zeros(len, 10);
kk = 1;
t = 0;

% 共有len个子样,每个采样时间段nn个子样,共len/nn个时间段,循环len/nn步
for k = 1:nn:len         
    t = t +nts;                             % t表示采样时间段
    [qnb, vn, pos] = insupdate(qnb, vn, pos, wm, vm, ts);
    vn(3) = 0;                              % 高度不变
    avp(kk,:) = [q2att(qnb); vn; pos; t]';  % 四元数转换为姿态角
    kk = kk +1;
    if mod(t, 500) < nts
        disp( fix(t) );                     % 显示进度
    end
end
avp(kk:end, :) = [];                        % 18000行之后设置为空
tt = avp(:, end);                           % tt为avp最后一列,时间

mysubplot(221, tt, avp(:, 1:2) /arcdeg, '\theta, \gamma /\circ');
mysubplot(222, tt, avp(:, 3) /arcdeg, '\psi /\circ');
mysubplot(223, tt, avp(:, 4:6), 'v^n / m/s');
mysubplot(224, tt, deltapos(avp(:, 7:9)), '\Deltap / m');

现在回头看看捷联惯导仿真,其实也没那么难,主线可以这样:
初始化 → 仿真静态IMU数据 → 更新导航信息 → 绘图 \text{初始化} \xrightarrow{} \text{仿真静态IMU数据} \xrightarrow{} \text{更新导航信息} \xrightarrow{} \text{绘图} 初始化 仿真静态IMU数据 更新导航信息 绘图
只不过需要细化来看。

(1)初始化

设置子样数、采样时间;
姿态、速度、位置初始化不必多说,还要根据姿态求四元数,注意这是从机体系到导航系的四元数表示;
地球导航参数计算,主要是为了得到导航系相对惯性系的角速度,当然这里是角速度增量,另外得到有害加速度的速度增量。

(2)仿真静态IMU数据

惯性器件静态,涉及到的角速度就是随地球自转的角速度,比力为重力矢量逆向,因此将导航系中的角速度速度通过四元数(注意qconj(qnb))换算到机体系角速度并乘时间得到角度增量,同样获得速度增量;
添加了失准角误差,并将四元数转换为考虑失准角误差的情况。

(3)更新导航信息

这是该程序中最为核心的部分,其中还包含对等效旋转矢量的圆锥误差补偿、对速度的旋转与划船误差的补偿;
更新中要注意坐标系的关系,例如将补偿后的速度增量dvbm先转换到导航系,在进行速度更新。

(4)绘图

此处略去十万字!


如果你比较懒,我把代码准备好了静态惯性器件捷联惯导仿真-资源汇总页中下载
如果你的积分比较多,快过期了,可以到这里消费下载0.0静态惯性器件捷联惯导仿真


从开始学习惯导到现在,一直在围绕这个程序进行,今天终于大体弄明白了,心情愉悦万分。这一个半月以来,从对惯导的一无所知到入门水平,就像当初爬泰山时翻过十八盘一样畅快—狠狠地迈上了一大步(如果把学会惯导的精髓同登上泰山比较,那我这个比喻太不恰当了哈哈)!
自从高中毕业之后,这也是少有的如此全神贯注地学习,这让我更加坚信环境是多么重要,让我更加坚定了当初自己的选择!
对于惯导的学习,还是不能放松,感觉距离进行惯导的研究还比较远,所以下一步打算把讲义看一遍,算是查漏补缺,当然还要尝试自己写程序,并进行某些算法的研究。
马上中秋了,估计还是舍不得离开办公室一天!现在终于体会到惜时如金的感受!希望十月一给自己放一天假,放松一下!
随便写写感受,反正也没人知道我是谁哈哈!!

align/aligncmps.m , 2830 align/alignfn.m , 2902 align/aligni0.m , 3339 align/alignsb.m , 1091 align/alignvn.m , 3267 align/alignWahba.m , 1575 base0/a2caw.m , 638 base0/a2cwa.m , 614 base0/a2mat.m , 704 base0/a2qua.m , 1039 base0/a2qua1.m , 782 base0/aa2mu.m , 1548 base0/aa2phi.m , 1546 base0/aaddmu.m , 1299 base0/askew.m , 577 base0/blh2xyz.m , 999 base0/cros.m , 861 base0/d2r.m , 393 base0/datt2mu.m , 606 base0/dm2r.m , 954 base0/dms2r.m , 1107 base0/dv2atti.m , 1170 base0/iaskew.m , 604 base0/lq2m.m , 577 base0/m2att.m , 551 base0/m2qua.m , 1203 base0/m2rv.m , 1320 base0/m2rv1.m , 1433 base0/m2rv2.m , 956 base0/mnormlz.m , 630 base0/mupdt.m , 1047 base0/p2cne.m , 675 base0/q2att.m , 916 base0/q2att1.m , 1119 base0/q2mat.m , 843 base0/q2rv.m , 643 base0/qaddafa.m , 715 base0/qaddphi.m , 754 base0/qconj.m , 417 base0/qdelafa.m , 671 base0/qdelphi.m , 754 base0/qmul.m , 692 base0/qmulv.m , 1760 base0/qnormlz.m , 416 base0/qq2afa.m , 693 base0/qq2phi.m , 782 base0/qupdt.m , 1119 base0/qupdt2.m , 2337 base0/r2d.m , 393 base0/r2dm.m , 885 base0/r2dms.m , 983 base0/rotv.m , 1198 base0/rq2m.m , 577 base0/rv2m.m , 1139 base0/rv2q.m , 824 base0/sv2atti.m , 1114 base0/vnormlz.m , 404 base0/xyz2blh.m , 1053 base1/altfilt.m , 1554 base1/attsyn.m , 1729 base1/bhsimu.m , 1299 base1/cnscl.m , 2895 base1/cnscl0.m , 2040 base1/conecoef.m , 1373 base1/conedrift.m , 1399 base1/conepolyn.m , 1116 base1/conesimu.m , 1442 base1/conetwospeed.m , 1200 base1/drinit.m , 1063 base1/drupdate.m , 1074 base1/dsins.m , 1185 base1/earth.m , 1484 base1/ethinit.m , 803 base1/ethupdate.m , 2104 base1/fusion.m , 971 base1/gcctrl.m , 1053 base1/gpssimu.m , 2747 base1/imulever.m , 646 base1/imurfu.m , 1998 base1/imurot.m , 621 base1/insextrap.m , 599 base1/insinit.m , 2232 base1/inslever.m , 969 base1/insupdate.m , 2257 base1/invbc.m , 577 base1/la2dpos.m , 901 base1/odsimu.m , 2475 base1/olsins.m , 798 base1/pp2vn
评论 13
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值