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

最新推荐文章于 2024-05-17 18:09:22 发布

嘀嗒zxy

最新推荐文章于 2024-05-17 18:09:22 发布

阅读量1.3k

点赞数 23

分类专栏：惯导文章标签：学习笔记 matlab

本文链接：https://blog.csdn.net/weixin_57967772/article/details/136903228

版权

惯导专栏收录该内容

5 篇文章 3 订阅

订阅专栏

PSINS工具箱学习与使用

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

简介

PSINS工具箱目录
由于初学，我刚接触到的是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，高度上的误差发散。
asp对比图
下面是进行高度补偿后:
asp对比图（添加了高度补偿）

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);