Psins代码解析之全局变量轨迹仿真(test_SINS_trj.m)惯性解算(test_SINS.m)
旋转椭球体的4个基本参数:长半轴、扁率(椭圆度)、地心引力常数、自转角速率;
以上内容来自:《车载定位定向系统关键技术研究》付强文
旋转椭球体:
地球自转角速度:
地球重力加速度为:
子午圈和卯酉圈曲率半径为:
以上内容来自:《捷联惯导算法及车载组合导航系统研究(硕士论文)》严恭敏
一、全局变量
global glvif ~exist('Re', 'var'), Re = []; endif ~exist('f', 'var'), f = []; endif ~exist('wie', 'var'), wie = []; endif isempty(Re), Re = 6378137; endif isempty(f), f = 1/298.257; endif isempty(wie), wie = 7.2921151467e-5; endglv.Re = Re; % the Earth's semi-major axisglv.f = f; % flatteningglv.Rp = (1-glv.f)*glv.Re; % semi-minor axisglv.e = sqrt(2*glv.f-glv.f^2); glv.e2 = glv.e^2; % 1st eccentricityglv.ep = sqrt(glv.Re^2-glv.Rp^2)/glv.Rp; glv.ep2 = glv.ep^2; % 2nd eccentricityglv.wie = wie; % the Earth's angular rateglv.meru = glv.wie/1000; % milli earth rate unitglv.g0 = 9.7803267714; % gravitational forceglv.mg = 1.0e-3*glv.g0; % milli gglv.ug = 1.0e-6*glv.g0; % micro g 为 微gglv.mGal = 1.0e-3*0.01; % milli Gal = 1cm/s^2 ~= 1.0E-6*g0glv.ugpg2 = glv.ug/glv.g0^2; % ug/g^2glv.ws = 1/sqrt(glv.Re/glv.g0); % Schuler frequencyglv.ppm = 1.0e-6; % parts per million百万分之一glv.deg = pi/180; % arcdeg 弧度单位 radglv.min = glv.deg/60; % arcminglv.sec = glv.min/60; % arcsecglv.hur = 3600; % time hour (1hur=3600second)glv.dps = pi/180/1; % arcdeg / secondglv.dph = glv.deg/glv.hur; % arcdeg / hour ;最终单位为 rad/s;
%比如:imuerr.eb(1:3) = 1*glv.dph;即为:((1*pi/180)/3600)rad/sglv.dpss = glv.deg/sqrt(1); % arcdeg / sqrt(second) 为 rad/sqrt(s)glv.dpsh = glv.deg/sqrt(glv.hur); % arcdeg / sqrt(hour) 为 rad/sqrt(hour);最终单位为:rad/sqrt(s)
%imuerr.web(1:3) = web*glv.dpsh;即为:((1*pi/180)/sqrt(3600))rad/s glv.dphpsh = glv.dph/sqrt(glv.hur); % (arcdeg/hour) / sqrt(hour) 为 rad/hour/sqrt(hour)glv.Hz = 1/1; % Hertzglv.dphpsHz = glv.dph/glv.Hz; % (arcdeg/hour) / sqrt(Hz) 为 rad/hour/sqrt(Hz)glv.ugpsHz = glv.ug/sqrt(glv.Hz); % ug / sqrt(Hz)glv.ugpsh = glv.ug/sqrt(glv.hur); % ug / sqrt(hour)glv.mpsh = 1/sqrt(glv.hur); % m / sqrt(hour)glv.mpspsh = 1/1/sqrt(glv.hur); % (m/s) / sqrt(hour), 1*mpspsh~=1700*ugpsHzglv.ppmpsh = glv.ppm/sqrt(glv.hur); % ppm / sqrt(hour)glv.mil = 2*pi/6000; % milglv.nm = 1853; % nautical mile 海里glv.kn = glv.nm/glv.hur; % knot 节%%glv.wm_1 = [0,0,0]; glv.vm_1 = [0,0,0]; % the init of previous gyro & acc sampleglv.cs = [ % coning & sculling compensation coefficients[2, 0, 0, 0, 0 ]/3[9, 27, 0, 0, 0 ]/20[54, 92, 214, 0, 0 ]/105[250, 525, 650, 1375, 0 ]/504 [2315, 4558, 7296, 7834, 15797]/4620 ];glv.csmax = size(glv.cs,1)+1; % max subsample numberglv.v0 = [0;0;0]; % 3x1 zero-vectorglv.qI = [1;0;0;0]; % identity quaternion 初始四元数glv.I33 = eye(3); glv.o33 = zeros(3); % identity & zero 3x3 matricesglv.pos0 = [34.246048*glv.deg; 108.909664*glv.deg; 380]; % position of INS Lab@NWPUglv.eth = []; glv.eth = earth(glv.pos0);%%[glv.rootpath, glv.datapath, glv.mytestflag] = psinsenvi;glv1 = glv;
二、生成仿真轨迹 test_SINS_trj.m
俯仰角抬头为正;横滚角右倾斜为正;
其中方位角变化时(转弯时),先将飞机滚转相应的角度;即:先滚转、后转弯;
已知航向角速率、前向速度,利用向量叉乘得到向心力;利用如下公式,计算出滚转角、滚转角速率、
代码实现:
%‘2’为航向角速率(转弯),2deg/s,转弯时间为45秒;
%协调转弯时间为4秒,即飞机横滚时间,很滚角速率 根据前向速度和航向角速率计算出
seg = trjsegment(seg, 'coturnleft', 45, 2, xxx, 4); trjsegment子函数中对应部分:case 'coturnleft', % coordinate turn leftrolllasting = var1; rollw = atan(cf/9.8)/dps/rolllasting; %rollw为横滚角速率(deg/s)seg = trjsegment(seg, 'rollleft', rolllasting, rollw);seg = trjsegment(seg, 'turnleft', lasting, w);seg = trjsegment(seg, 'rollright', rolllasting, rollw);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]];case '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]];
描述一段完整的运动轨迹,其中初始姿态为0、初始速度为0、初始位置为【34.246048; 108.909664; 380】
- 初始化
- 静止100秒
- 向北加速飞行10秒(a=1m/s^2)
- 匀速保持100秒
- 左转弯90°(最终方向为正西,包括飞机先滚转、再转弯、最后反向滚转,恢复滚转角为0)
- 匀速保持100秒
- 右转弯450°(最终方向为正北,包括飞机先滚转、再转弯、最后反向滚转,恢复滚转角为0)
- 匀速保持100秒
- climb 抬头(2°/s *10s=20°)-匀速保持-低头(-2°/s *10s=-20°)
- 匀速保持100秒
- descent 低头(-2°/s *10s=-20°) -保持 -抬头(2°/s *10s=20°)
- 匀速保持100秒
- 减速 5秒(a=-2m/s^2),至速度为0
- 静止100秒
通过轨迹可以看出,最终:roll=pitch=0;yaw=0°;飞机朝北飞行;飞机速度矢量为0矢量;轨迹仿真时间为:966秒
航向角:
水平姿态角:
存在问题地方:生成角增量和速度增量的公式:
三、纯惯性导航仿真 test_SINS.m
由于纯惯导解算高度通道发散;
1、IMU误差生成函数和IMU数据添加误差函数:imuerrset.m和imuadderr.m
此处主要添加两项误差:常值零偏和随机游走为例:
(1)imuerrset.m
%% constant bias & random walk
imuerr.eb(1:3) = eb*glv.dph; imuerr.web(1:3) = web*glv.dpsh;
imuerr.db(1:3) = db*glv.ug; imuerr.wdb(1:3) = wdb*glv.ugpsHz;
最终:
imuerr.eb单位为:rad/s ; imuerr.web单位为:rad/sqrt(s)
imuerr.db单位为:m/s^2 ; imuerr.wdb单位为:m/s^2/sqrt(s)
(2)imuadderr.m
drift = [ ts*imuerr.eb(1) + sts*imuerr.web(1)*randn(m,1), ...ts*imuerr.eb(2) + sts*imuerr.web(2)*randn(m,1), ...ts*imuerr.eb(3) + sts*imuerr.web(3)*randn(m,1), ...ts*imuerr.db(1) + sts*imuerr.wdb(1)*randn(m,1), ...ts*imuerr.db(2) + sts*imuerr.wdb(2)*randn(m,1), ...ts*imuerr.db(3) + sts*imuerr.wdb(3)*randn(m,1) ];
imu(:,1:6) = imu(:,1:6) + drift;
2、初始姿态、速度、位置误差
avpseterr.m
function davp = avpseterr(phi, dvn, dpos)
% avp errors setting.
% Inputs: phi - platform misalignment angles. NOTE: leveling errors
% phi(1:2) in arcsec, azimuthe error phi(3) in arcmin
% dvn - velocity errors in m/s
% dpos - position errors dpos=[dlat;dlon;dhgt], all in m
% Output: davp = [phi; dvn; dpos]
3、利用一阶马尔可夫生成气压高度计仿真信息
bhsimu.m
t = (trj.avp(1,10):ts:trj.avp(end,10))';bh = interp1(trj.avp(:,10), trj.avp(:,9), t, 'linear');bh = bh + bias + markov1(var, tau, ts, length(bh),1);
一阶马尔可夫过程生成的数据为:
利用一阶马尔可夫+初始高度偏差+真实高度=气压高度计,结果为;
目的是每次捷联惯导更新时,都将气压高度计送到位置信息中;
ins.vn(3) = href(k1,2); ins.pos(3) = href(k1,1);
4、纯惯导解算:
(1)首先对地球、载体相关参数进行外推以双子样为例,nts=2*ts,外推ts;
%% earth & angular rate updating vn01 = ins.vn+ins.an*nts2; pos01 = ins.pos+ins.Mpv*vn01*nts2; % extrapolation at t1/2ins.eth = ethupdate(ins.eth, pos01, vn01);ins.wib = phim/nts; ins.fb = dvbm/nts; % same as trjsimuins.web = ins.wib - ins.Cnb'*ins.eth.wnie;
% ins.wnb = ins.wib - ins.Cnb'*ins.eth.wnin;ins.wnb = ins.wib - (ins.Cnb*rv2m(phim/2))'*ins.eth.wnin; % 2014-11-30 因为是外推1/2时刻,所以除以2
主要是利用:
- 前一时刻速度、加速度、前一时刻位置;外推得到nts时刻的速度、位置
- 利用外推得到的速度、位置去更新地球相关参数,主要为:子午圈、卯酉圈半径;东北天下wnie分量(与纬度有关)、wnen分量(与速度、位置有关)、重力加速度(与纬度、高度有关)、等信息
(2)速度更新:以双子样为例,nts=2*ts
程序中,首先根据子样数进行圆锥补偿,得到dvbm,即速度增量;
nn = size(imu,1);nts = nn*ins.ts; nts2 = nts/2; ins.nts = nts;[phim, dvbm] = cnscl(imu,0); % coning & sculling compensation
% [phim, dvbm] = cnscl0(imu); % coning & sculling compensationphim = ins.Kg*phim-ins.eb*nts; dvbm = ins.Ka*dvbm-ins.db*nts; % calibration
然后根据速度更新公式:
其中速度更新公式中的一项如下:dvbm/Δt就等于括号中的内容,也就是下面的 ins.fb
也就是ins.fn=qmulv(ins.qnb,ins.fb);然后通过旋转矢量进行变换;
%% (1)velocity updatingins.fn = qmulv(ins.qnb, ins.fb);
% ins.an = qmulv(rv2q(-ins.eth.wnin*nts2),ins.fn) + ins.eth.gcc;ins.an = rotv(-ins.eth.wnin*nts2, ins.fn) + ins.eth.gcc;vn1 = ins.vn + ins.an*nts;
(3)位置更新:以双子样为例,nts=2*ts
利用上面外推nts时刻得到的地球参数和速度更新得到的速度;利用梯形积分得到位置;
%% (2)position updating
% ins.Mpv = [0, 1/ins.eth.RMh, 0; 1/ins.eth.clRNh, 0, 0; 0, 0, 1];ins.Mpv(4)=1/ins.eth.RMh; ins.Mpv(2)=1/ins.eth.clRNh;
% ins.Mpvvn = ins.Mpv*((ins.vn+vn1)/2+(ins.an-ins.an0)*nts^2/3); % 2014-11-30ins.Mpvvn = ins.Mpv*(ins.vn+vn1)/2;ins.pos = ins.pos + ins.Mpvvn*nts; ins.vn = vn1;ins.an0 = ins.an;
(4)姿态更新:以双子样为例,nts=2*ts
%% (3)attitude updatingins.Cnb0 = ins.Cnb;
% ins.qnb = qupdt(ins.qnb, ins.wnb*nts); % lower accuracy than next lineins.qnb = qupdt2(ins.qnb, phim, ins.eth.wnin*nts);[ins.qnb, ins.att, ins.Cnb] = attsyn(ins.qnb);ins.avp = [ins.att; ins.vn; ins.pos];
利用四元数进行姿态更新;
其中用到了三角函数和单位四元数关系:
5、误差图:
Psins代码解析之全局变量轨迹仿真(test_SINS_trj.m)惯性解算(test_SINS.m)相关推荐
- Psins代码解析之静基座仿真(test_SINS_static.m)傅科、修拉周期水平通道误差传播(test_SINS_static_verify.m)
静基座条件下:速度矢量为0:地理位置精确已知: 一.静基座下,陀螺仪和加速度计数据仿真: imustatic.m 重力矢量和地球自转角速度在地理坐标系下: 上式中,Wnen为0,Wnnb为0,即静基座 ...
- Psins代码解析之kalman松组合导航融合算法 test_SINS_GPS_153.mtest_SINS_GPS_186.mtest_SINS_GPS_193.m
框架: 设置松组合导航算法中状态量.观测量数目:比如:psinstypedef(153),状态误差量为15维,量测量为3维: 对仿真生成的飞行轨迹(test_SINS_trj.m),设置IMU误差:并 ...
- Psins代码解析之线性误差模型精度验证(test_SINS_error_model_verify.m)
捷联惯导误差方程的推导前提是: 惯性传感器误差模型 前提是:IMU设备经过出厂标定之后,对所谓IMU传感器存在的"残差"的考虑:主要是:失准角.非正交角.零偏.刻度系数.二次项误差 ...
- Psins代码解析之常用的子函数
1.insinit.m 惯导参数结构体初始化(SINS structure array initialization). % Prototype: ins = insinit(avp0, ts, va ...
- Psins代码解析之test_SINS_east_west.mtest_SINS_north_south.m
导航坐标系:东-北-天 载体坐标系:右-前-上 欧拉角定义:3-1-2旋转,(航向角-俯仰角-滚转角),其中航向角北偏西为正,范围[-pi pi] 地球自转引起的导航系旋转: 因地球表面弯曲,载体在地 ...
- PX4代码解析(6)
一.前言 上一节介绍了PX4姿态估计调用函数的流程,这一节分享一下我对PX4姿态解算的解读.首先,要理解PX4姿态解算的程序,要先从传感器的特性入手,这里主要介绍的传感器有加速度计,磁力计,陀螺仪. ...
- SORT4 SORT项目代码解析
SORT系列 SORT-1 项目配置运行-WINDOWS SORT-2 卡尔曼滤波推导和示例 SORT-3 匈牙利算法和SORT类 SORT-4 SORT项目代码解析 本项目地址 SORT项目逐层详解 ...
- python 全局变量使用报错没有定义_Python变量作用域代码解析
本篇文章小编给大家分享一下Python变量作用域代码解析,文章代码介绍的很详细,小编觉得挺不错的,现在分享给大家供大家参考,有需要的小伙伴们可以来看看. 特点 python的作用域是静态的,在源代码中 ...
- 【四足机器人--摆动相足端位置速度轨迹规划】(4.1)FootSwingTrajectory(bezier曲线计算脚的摆动轨迹)代码解析
系列文章目录 提示:这里可以添加系列文章的所有文章的目录,目录需要自己手动添加 TODO:写完再整理 文章目录 系列文章目录 前言 一.FootSwingTrajectory(bezier曲线)的内容 ...
最新文章
- JS根据两点的经纬度坐标得到驾车行驶距离
- Mybatis(三) 映射文件详解
- linux下wifi编程(基于netlink和nl80211.h)
- VTK:vtkAreaPicker用法实战
- java string 异或_Java源码——String
- 终于有人将进程间通信讲明白了
- apache2.2:使一个目录允许执行cgi程序
- JavaWeb笔记(七)FilterListener
- c语言case label,an enum switch case label must be the unqualified name of an enumeration constant
- POJ 2409 Let it Bead(Polya简单应用)
- HeadFirstJava——14_数据结构
- Android Performance之开机优化(1)-开机启动优化工具
- 前端移动端高度自适应
- Python爬虫|爬取喜马拉雅音频
- 计算机主机中包,百度地图脱机包最终可以在计算机上导入
- 联想G40-30进win10PE触摸板、键盘无反应
- iOS 如何判断静音开关是否打开
- 渗透测试流程(基础理论)
- 渗透测试-地基篇-美杜莎Medusa(十二)
- php获取今日、昨日、本周、本月 日期方法