前言

test_SINS_trj是航迹仿真程序，具体实现可以参考严恭敏老师的硕士论文《捷联惯导算法及车载组合导航系统研究》中第5章节中的第一小节。

关于航迹仿真

航迹仿真是根据设定的航迹生成惯性器件信息源（比力和角速度），并给出相应航迹点的航行参数（姿态、速度和位置）。比力和角速度用作捷联算法的数据输入，经过捷联算法解算后其结果可以和前述航行参数作比较。航迹仿真的方法是根据设定的航行运动状态解算一组航迹微分方程组，通常利用计算计求解该微分方程组的数值解，数值解法中常用到的是四阶龙格-库塔法。

代码解读

trjsegment函数中可以根据不同的动作以及动作参数生成相应的运动参数。
先说下结构体seg.wat 中8个参数的含义：

% wat(:,1) - period lasting time                 持续时间
% wat(:,2) - period initial velocity magnitude   速度大小
% wat(:,3:5) - angular rate in trajectory-frame  角速度 ,单位：rad/s
% wat(:,6:8) - acceleration in trajectory-frame  加速度

init：可以设定初始速度initvelocity；
uniform：可以设定按照上一时刻的速度，匀速直行，当速度为0时，就是设定可以静止多长时间；
accelerate：直线加速过程，可以设置加速度的大小和时间；
deaccelerate：直线减速过程，可以设置加速度的大小和时间；
headup：俯仰向上，此时速度不变，俯仰角变化到指定的值，可以看到seg.wat中第3个参数为俯仰角变化的角速度，改变过程中角速度是定值；
headdown：低垂向下，此时速度不变，俯仰角变化到指定的值，可以看到seg.wat中第3个参数为俯仰角变化的角速度，改变过程中角速度是定值；
turnleft：左转弯，只改变偏航角，偏航角改变过程中角速度是定值；
turnright：右转弯，只改变偏航角，偏航角改变过程中角速度是定值；
rollleft：这个动作就叫滚转左转弯吧，只改变滚转角，改变过程中角速度是定值；
rollright：这个动作就叫滚转右转弯吧，只改变滚转角，改变过程中角速度是定值；
coturnleft：论文中把这个动作叫做协调转弯，通俗点理解即是一个有转弯半径的转弯，该动作由3个动作组组成，具体叙述如下所示：

cf = (w*dps)*seg.vel; % centripetal force :向心力
rollw = atan(cf/9.8)/dps/rolllasting; % 滚转角(角速度)

从滚转角角速度可以看出，其由线速度和偏航角速度决定，trjsegment函数代码注释如下所示：

function seg = trjsegment(seg, segtype, lasting, w, a, var1)
% Add trjsegment setting for trajectory simulator.
%
% Prototype: seg = trjsegment(seg, segtype, lasting, w, a, var1)
% Inputs: seg - trjsegment structure array
%         segtype - trjsegment type
%         lasting - segment lasting time 持续时间
%         w - trajectory angular rate (NOTE: in deg/sec!) 角速度
%         a - trajectory acceleration  加速度
%         var1 - augmented input, see code in detail
% Output: seg - new trjsegment structure array
%
% See also  trjsimu, insupdate.% Copyright(c) 2009-2014, by Gongmin Yan, All rights reserved.
% Northwestern Polytechnical University, Xi An, P.R.China
% 07/01/2014%==============================================================%
% wat(:,1) - period lasting time                 持续时间
% wat(:,2) - period initial velocity magnitude   速度大小
% wat(:,3:5) - angular rate in trajectory-frame  角速度 ,单位：rad/s
% wat(:,6:8) - acceleration in trajectory-frame  加速度
%==============================================================%dps = pi/180/1;  % deg/second 转换为radif exist('w', 'var')cf = (w*dps)*seg.vel; % centripetal force :向心力 % 向心力公式如下：% https://baike.baidu.com/item/%E5%90%91%E5%BF%83%E5%8A%9B/975374endswitch(segtype)case 'init' % trjsegment(***, 'init', initvelocity)initvelocity = lasting; % 0seg = [];seg.vel = initvelocity;  seg.wat = [];case 'uniform'seg.wat = [seg.wat; [lasting, seg.vel, 0, 0, 0, 0, 0, 0]];case 'accelerate'seg.wat = [seg.wat; [lasting, seg.vel, 0, 0, 0, 0, a, 0]];seg.vel = seg.vel + lasting*a; % 加速过程case 'deaccelerate' % 减速seg.wat = [seg.wat; [lasting, seg.vel, 0, 0, 0, 0,-a, 0]];seg.vel = seg.vel - lasting*a;% 俯仰向上    case 'headup'seg.wat = [seg.wat; [lasting, seg.vel, w*dps, 0, 0, 0, 0, cf]];% 低垂向下case 'headdown'seg.wat = [seg.wat; [lasting, seg.vel,-w*dps, 0, 0, 0, 0,-cf]];% w*dps:rad% 偏航角速率保持定值case 'turnleft'seg.wat = [seg.wat; [lasting, seg.vel, 0, 0, w*dps,-cf, 0, 0]];case 'turnright'seg.wat = [seg.wat; [lasting, seg.vel, 0, 0,-w*dps, cf, 0, 0]];% 向左滚转% 滚转角速度由0变为某个值(y轴)% lasting: 转弯角速度% w:横滚角改变量,单位：deg/scase 'rollleft'seg.wat = [seg.wat; [lasting, seg.vel, 0,-w*dps, 0, 0, 0, 0]];% 向右滚转case 'rollright'seg.wat = [seg.wat; [lasting, seg.vel, 0, w*dps, 0, 0, 0, 0]];% compound ------case 'coturnleft'% coordinate turn left% cf = (w*dps)*seg.vel; % centripetal force :向心力 % dps = pi/180/1;  % deg/second 转换为rad% rolllasting：转弯角速度% rollw:横滚角角速度,单位：deg/s% 改变滚转角进入转弯阶段持续时间:roll lasting% 参考：硕士论文70页% w：偏航角角速度rolllasting = var1; rollw = atan(cf/9.8)/dps/rolllasting; % 滚转角(角速度)seg = trjsegment(seg, 'rollleft',  rolllasting, rollw);seg = trjsegment(seg, 'turnleft',  lasting, w);seg = trjsegment(seg, 'rollright', rolllasting, rollw);case 'coturnright' % coordinate turn rightrolllasting = var1; rollw = atan(cf/9.8)/dps/rolllasting;seg = trjsegment(seg, 'rollright', rolllasting, rollw);seg = trjsegment(seg, 'turnright', lasting, w);seg = trjsegment(seg, 'rollleft',  rolllasting, rollw);case '8turn'% w：deg/s% lasting：持续时间lasting = 360/w;rolllasting = var1;seg = trjsegment(seg, 'coturnleft',  lasting, w, 0, rolllasting);seg = trjsegment(seg, 'coturnright', lasting, w, 0, rolllasting);case 'sturn'lasting1 = 90/w; lasting2 = 180/w;rolllasting = var1;seg = trjsegment(seg, 'coturnright', lasting1, w, 0, rolllasting);seg = trjsegment(seg, 'coturnleft',  lasting2, w, 0, rolllasting);seg = trjsegment(seg, 'coturnright', lasting1, w, 0, rolllasting);seg = trjsegment(seg, 'coturnleft',  lasting1, w, 0, rolllasting);seg = trjsegment(seg, 'coturnright', lasting2, w, 0, rolllasting);seg = trjsegment(seg, 'coturnleft',  lasting1, w, 0, rolllasting);case 'climb'uniformlasting = var1;seg = trjsegment(seg, 'headup',   lasting, w);seg = trjsegment(seg, 'uniform',  uniformlasting);seg = trjsegment(seg, 'headdown', lasting, w);case 'descent'uniformlasting = var1;seg = trjsegment(seg, 'headdown', lasting, w);seg = trjsegment(seg, 'uniform',  uniformlasting);seg = trjsegment(seg, 'headup',   lasting, w);otherwiseerror('trjsegment type mismatch.');end

参考论文

捷联惯导算法及车载组合导航系统研究西北工业大学严恭敏

PSINS源码阅读—test_SINS_trj相关推荐

PSINS源码test_SINS_DR解析
PSINS源码test_SINS_DR解析前言源码解析整体介绍对源码参数的更改 test_SINS_DR脚本 glvf函数 drinit函数 odsimu函数 RMRN函数 imuerrset ...
应用监控CAT之cat-client源码阅读（一）
CAT 由大众点评开发的,基于 Java 的实时应用监控平台,包括实时应用监控,业务监控.对于及时发现线上问题非常有用.(不知道大家有没有在用) 应用自然是最初级的,用完之后,还想了解下其背后的原理, ...
centos下将vim配置为强大的源码阅读器
每日杂事缠身,让自己在不断得烦扰之后终于有了自己的清静时光来熟悉一下我的工具,每次熟悉源码都需要先在windows端改好,拖到linux端,再编译.出现问题,还得重新回到windows端,这个过程太耗 ...
源码阅读：AFNetworking（十六）——UIWebView+AFNetworking
该文章阅读的AFNetworking的版本为3.2.0. 这个分类提供了对请求周期进行控制的方法,包括进度监控.成功和失败的回调. 1.接口文件 1.1.属性 /**网络会话管理者对象*/ @prop ...
源码阅读：SDWebImage（六）——SDWebImageCoderHelper
该文章阅读的SDWebImage的版本为4.3.3. 这个类提供了四个方法,这四个方法可分为两类,一类是动图处理,一类是图像方向处理. 1.私有函数先来看一下这个类里的两个函数 /**这个函数是计算 ...
mybatis源码阅读
说下mybatis执行一个sql语句的流程执行语句,事务等SqlSession都交给了excutor,excutor又委托给statementHandler SimpleExecutor:每执行一次 ...
24 UsageEnvironment使用环境抽象基类——Live555源码阅读(三)UsageEnvironment
24 UsageEnvironment使用环境抽象基类--Live555源码阅读(三)UsageEnvironment 24 UsageEnvironment使用环境抽象基类--Live555源码阅读 ...
Transformers包tokenizer.encode()方法源码阅读笔记
Transformers包tokenizer.encode()方法源码阅读笔记_天才小呵呵的博客-CSDN博客_tokenizer.encode
源码阅读笔记 BiLSTM+CRF做NER任务流程图
源码阅读笔记 BiLSTM+CRF做NER任务(二) 源码地址:https://github.com/ZhixiuYe/NER-pytorch 本篇正式进入源码的阅读,按照流程顺序,一一解剖. 一.流 ...

PSINS源码阅读—test_SINS_trj

文章目录

前言

关于航迹仿真

代码解读

参考论文

PSINS源码阅读—test_SINS_trj相关推荐

最新文章

热门文章