严恭敏老师PSINS工具箱学习笔记-1

PSINS工具箱学习与使用

刚开始入门惯性导航算法，看了一些书但实践出了一些问题，经推荐了解到西工大严恭敏老师的PSINS工具箱很适合自学，就在网上找了一些相关资料，很全。
网址：http://www.psins.org.cn/sy
b站介绍：https://www.bilibili.com/video/BV1R54y1E7ut/?vd_source=6ce8821b81ac808150f82236f5c1f721

简介

由于初学，我刚接触到的是base、data、demos文件夹：

• base里面放了一些必要的函数，b站视频里严老师讲编了大概有五百多个，其中一百多个较为常用；
• data存放一些数据；
• demos是一些比较经典的例子。

初始化和大概了解

psinsinit.m——安装工具箱，初始化
psinsmain.m——演示很多例子功能
上面两个文件的演示严老师视频里面说得很清楚了，初学者还用不太上。

test_SINS

这是刚开始学接触到的第一个一系列捷联惯导的例程，在demo文件夹下。

test_SINS_trj.m

自行产生一些avp信息以及角速度和比力值。

• trj–trajectory轨迹
• AVP，attitude，velocity，position

% Trajectory generation for later simulation use.
% See also  test_SINS, test_SINS_GPS_153, test_DR.
% Copyright(c) 2009-2014, by Gongmin Yan, All rights reserved.
% Northwestern Polytechnical University, Xi An, P.R.China
% 10/06/2011, 10/02/2014
glvs 
ts = 0.1;       % sampling interval
avp0 = [[0;0;0]; [0;0;0]; glv.pos0]; % init avp
% trajectory segment setting
xxx = [];
seg = trjsegment(xxx, 'init',         0);
seg = trjsegment(seg, 'uniform',      100);
seg = trjsegment(seg, 'accelerate',   10, xxx, 1);
seg = trjsegment(seg, 'uniform',      100);
seg = trjsegment(seg, 'coturnleft',   45, 2, xxx, 4);
seg = trjsegment(seg, 'uniform',      100);
seg = trjsegment(seg, 'coturnright',  10*5, 9, xxx, 4);
seg = trjsegment(seg, 'uniform',      100);
seg = trjsegment(seg, 'climb',        10, 2, xxx, 50);
seg = trjsegment(seg, 'uniform',      100);
seg = trjsegment(seg, 'descent',      10, 2, xxx, 50);
seg = trjsegment(seg, 'uniform',      100);
seg = trjsegment(seg, 'deaccelerate', 5,  xxx, 2);
seg = trjsegment(seg, 'uniform',      100);
% generate, save & plot
trj = trjsimu(avp0, seg.wat, ts, 1);
trjfile('trj10ms.mat', trj);
insplot(trj.avp);
imuplot(trj.imu);
pos2gpx('trj_SINS_gps', trj.avp(1:round(1/trj.ts):end,7:9)); % to Google Earth

• trj：轨迹结构体，imu和avp为其中的两个cell。imu，7列数据，最后一列是时间；avp，10列数据，最后一列是时间。
• glv：结构体-定义的全局变量，几乎每个.m文件都会进行全局定义一遍，函数： glvs，定义了很多常量，可在base文件夹下找到，或者直接在代码中转到glvs，几乎所有函数都可以这种方式查询。
• trjsegment：函数，每个segment可自行定义。
• trjsimu：函数，轨迹模拟，输入初始位置和每个segment的状态，产生时间间隔，重复次数。trj结构体里面有很多cell，可以打开工作区查看。
• trjfile：函数，将trj保存.mat文件可直接加载，后续仿真都要提前load这些数据。
• insplot：工具箱里好用的画图函数，画出avp信息，b站视频也有严老师的详细讲解。
• imuplot：同上insplot，画出gyros和acc的测量值。

test_SINS.m

纯惯导解算avp信息，就是根据 trj10ms.mat里面imu测量值和avp0初始信息，去解算avp。

% SINS pure inertial navigation simulation. Please run 
% 'test_SINS_trj.m' to generate 'trj10ms.mat' beforehand!!!
% See also  test_SINS_trj, test_SINS_GPS_153, test_SINS_static.
% Copyright(c) 2009-2014, by Gongmin Yan, All rights reserved.
% Northwestern Polytechnical University, Xi An, P.R.China
% 17/06/2011
glvs
trj = trjfile('trj10ms.mat');
%% error setting
imuerr = imuerrset(0.01, 100, 0.001, 10);
imu = imuadderr(trj.imu, imuerr);
davp0 = avperrset([0.5;0.5;5], 0.1, [10;10;10]);
avp00 = avpadderr(trj.avp0, davp0); 
trj = bhsimu(trj, 1, 10, 3, trj.ts);
%% pure inertial navigation & error plot
avp = inspure(imu, avp00, trj.bh, 1);
% avp = inspure(imu, avp00, 'f', 1);% 'f' - height free.
avperr = avpcmpplot(trj.avp, avp);

• 加载数据
• 误差设置，errset，adderr分别为设置值，加入误差值（imu，avp皆适用）。
  imuerr——输入：【gyros，acc常数偏移，gyros，acc随机偏移（噪声）】
  avperr——角度上通常yaw角设置误差较大，pitch和roll较小；速度上三个方向一致；位置上也是一致。
  bhsimu，高度误差补偿，还未涉及，初步了解到是为了补偿位置上的高度。
• 解算avp，inspure纯惯导解算。输入imu测量数据，avp初始值，以及模式（具体见函数）等。
• 做图对比，inspure会绘图解算的avp信息，avpcmpplot画出对比以及误差图。

davp0 = avperrset([0.5;0.5;5], 0.1, [10;10;10]); % 初始asp误差设置
imuerr = imuerrset(0.01, 100, 0.001, 10);% IMU误差设置

下面是未进行高度补偿，可以看到图6，高度上的误差发散。

下面是进行高度补偿后:

trj = trjsimu(avp0, seg.wat, ts, 3);% 将循环次数改为3，即可得到3000s的一个轨迹

高度补偿再加上更长时间的数据，此时位置误差是收敛的。
（ps：图1纵坐标单位为°，图2纵坐标为 ’ [errset里面角度误差的单位为glv.min] ）

经过图片对比，可以观察到avp速度误差会随时间增大而增大，角度误差与初始位置有关，imuerrset的误差远小于初始误差，位置反而在一定时间内会收敛（某些误差因素抵消的原因？）。

test_SINS_GPS_153.m

将惯导和卫导组合，使用卡尔曼滤波进行滤波。此处涉及到卡尔曼滤波器，工具包将其写入kfupdate函数中，并且定义kf结构体，将滤波中所需要的矩阵都包含了。
文件命名15代表状态维数，3代表测量维数。
此处状态为：

• 三个姿态欧拉角（失准角？老师视频里说的不知道是哪几个字）
• 三个速度误差
• 三个位置误差
• 三个陀螺仪常值漂移
• 三个加速度计常值漂移

18维添加了三个卫导和惯导之间距离误差；19维添加了时间不同步误差。

1.代码初始化部分

• 全局变量定义，滤波器的维数定义
• 采样时间处理
• 误差添加
• 初始化ins

glvs
psinstypedef(153);
trj = trjfile('trj10ms.mat');
% initial settings
[nn, ts, nts] = nnts(2, trj.ts);
imuerr = imuerrset(0.03, 100, 0.001, 5);
imu = imuadderr(trj.imu, imuerr);
davp0 = avperrset([0.5;-0.5;20], 0.1, [1;1;3]);
ins = insinit(avpadderr(trj.avp0,davp0), ts);

2.卡尔曼滤波部分

• kfinit函数，根据ins输入，初始avp，imu测量误差以及初始位置误差初始化，见下，设置Qt、Rk、Pxk、Hk（imuerr有一系列的默认误差值）。
• kfupdate函数，根据输入值，可选择是状态更新还是测量更新。
• kf结构体包含很多滤波过程中需要的数据。

% KF filter
rk = poserrset([1;1;3]);% rk为卫导位置测量误差
kf = kfinit(ins, davp0, imuerr, rk);
% 设置最小协方差矩阵
kf.Pmin = [avperrset(0.01,1e-4,0.1); gabias(1e-3, [1,10])].^2;  kf.pconstrain=1;
len = length(imu); [avp, xkpk] = prealloc(fix(len/nn), 10, 2*kf.n+1);
timebar(nn, len, '15-state SINS/GPS Simulation.'); 
ki = 1;
% 进入滤波
for k=1:nn:len-nn+1
    k1 = k+nn-1;  
    wvm = imu(k:k1,1:6);  t = imu(k1,end);
    ins = insupdate(ins, wvm);
    kf.Phikk_1 = kffk(ins);%定义状态转移矩阵
    kf = kfupdate(kf);
    if mod(t,1)==0 %ins间隔0.2s，gps间隔1s
        posGPS = trj.avp(k1,7:9)' + davp0(7:9).*randn(3,1);  % GPS pos simulation with some white noise
        kf = kfupdate(kf, ins.pos-posGPS, 'M'); 
        [kf, ins] = kffeedback(kf, ins, 1, 'avp');%反馈补偿
        avp(ki,:) = [ins.avp', t];
        xkpk(ki,:) = [kf.xk; diag(kf.Pxk); t]';  ki = ki+1;
    end
    timebar;
end
avp(ki:end,:) = [];  xkpk(ki:end,:) = []; 
% show results
insplot(avp);
avperr = avpcmpplot(trj.avp, avp);
kfplot(xkpk, avperr, imuerr);

根据imu测量值和ins初始状态更新ins：

ins = insupdate(ins, wvm);

输入只有kf时，函数状态更新。

kf = kfupdate(kf);

输入有测量值时，且字符串为M，为测量更新。

kf = kfupdate(kf, ins.pos-posGPS, 'M'); 

结果图分析，ASP误差明显优于纯惯导，估计出来的状态值也与真实值（实际中是不知道的，仿真的误差是实验设置的）相吻合。

总结

通过这三个程序，可以实现生成仿真ASP数据，IMU测量数据；纯惯导仿真；卫导和惯导组合仿真。