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

主函数

%捷联惯导仿真主程序
%东北天坐标系，东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');

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

(1)初始化

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

（2）仿真静态IMU数据

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

（3）更新导航信息

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

（4）绘图

此处略去十万字！

如果你比较懒，我把代码准备好了静态惯性器件捷联惯导仿真密码：843u
如果你的积分比较多，快过期了，可以到这里消费下载0.0静态惯性器件捷联惯导仿真

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