函数[nav_e] = ins_gnss (imu, gnss att_mode)
% ins_gnss:松散耦合的组合导航系统。
%
% ins_gnss IMU和GNSS测量集成通过使用一个扩展的卡尔曼滤波器。
%
%的输入
% imu: imu数据结构。
% t: Ix1向量时间(秒)。
% fb: Ix3加速度向量车身骨架XYZ (m / s ^ 2)。
%斌:Ix3转率向量车身骨架XYZ(弧度/秒)。
% arw: 1 x3角随机漫步(rad / s / root-Hz)。
% vrw: 1 x3角行走速度(米/秒^ 2 / root-Hz)。
% gstd: 1 x3陀螺仪标准差(弧度/秒)。
%、美国:1 x3 accrs标准差(m / s ^ 2)。
% gb_sta: 1 x3陀螺静态偏差或刺激偏见(弧度/秒)。
% ab_sta: 1 x3 accrs静态偏差或刺激偏见(m / s ^ 2)。
% gb_dyn: 1 x3陀螺仪动态偏差或偏见不稳定(弧度/秒)。
% ab_dyn: 1 x3 accrs动态偏差或偏见不稳定(m / s ^ 2)。
% gb_corr: 1 x3陀螺相关时间(秒)。
% ab_corr: 1 x3 accrs相关时间(秒)。
% gb_psd: 1 x3陀螺仪动态偏差PSD (rad / s / root-Hz)。
% ab_psd: 1 x3 accrs动态偏差PSD (m / s ^ 2 / root-Hz);
%频率:1 x1采样频率(赫兹)。
% ini_align: 1 x3初始t(1)的态度。
% ini_align_err: 1 x3初始t(1)错误的态度。
%
% gnss: gnss的数据结构。
% t: mx₁向量时间(秒)。
% lat: mx₁纬度(弧度)。
%经度:mx₁经度(弧度)。
% h: mx₁高度(米)。
%韦尔:Mx3 NED速度(米/秒)。
% std: 1 x3位置标准差(rad, rad, m)。
%澳门旅游娱乐有限公司:1 x3位置标准差(m, m, m)。
% stdv: 1 x3速度标准差(米/秒)。
% larm: 3 x1杠杆臂从IMU GNSS天线(y, x-fwd z-down) (m)。
%频率:1 x1采样频率(赫兹)。
% zupt_th: 1 x1 ZUPT阈值(米/秒)。
% zupt_win: 1 x1 ZUPT时间窗口(秒)。
%每股收益:1 x1时间间隔比较当前IMU当前GNSS时间向量(s)。
%
% att_mode:态度模式字符串。
%“四元数”:在四元数姿态更新格式。默认值。
%的dcm:态度直接余弦矩阵格式的更新。
%
%输出
% nav_e: INS / GNSS导航估计数据结构。
% t: Ix1 INS向量时间(秒)。
% tg: mx₁GNSS时间向量,当卡尔曼滤波器被处决(秒)。
%辊:Ix1辊(弧度)。
%情节:Ix1音高(弧度)。
%偏航:Ix1偏航(弧度)。
%韦尔:Ix3 NED速度(米/秒)。
% lat: Ix1纬度(弧度)。
%经度:Ix1经度(弧度)。
% h: Ix1高度(m)。
% xi: Mx15卡尔曼滤波器的状态。
% xp: Mx15卡尔曼滤波器后验状态。
% z: Mx6 INS / GNSS测量
% v: Mx6卡尔曼滤波器的创新。
% b: Mx6卡尔曼滤波器偏差补偿,[gb_dyn ab_dyn]。
%:Mx225卡尔曼滤波器过渡态矩阵,每一个矩阵
%行命令列。
%页:Mx225卡尔曼滤波器后验协方差矩阵,一个
%矩阵每一行命令列。
%π:Mx225卡尔曼滤波器先验协方差矩阵,一个矩阵
%每行命令列。
% ins_gps。米,ins_gnss函数是基于之前的函数。
%
如果输入参数个数< 3,att_mode =“四元数”;结束
% % ZUPT检测算法
zupt = false;
% %预先配置
%常数矩阵
我眼睛= (3);
O = 0 (3);
%的INS时间长度的向量
李=长度(imu.t);
%的GNSS时间长度的向量
LG =长度(gnss.t);
%的态度
李roll_e = 0 (1);
李pitch_e = 0 (1);
李yaw_e = 0 (1);
% yawm_e = 0 (Mi, 1);
%初始化估计在INS时间= 1
roll_e (1) = imu.ini_align (1);
pitch_e (1) = imu.ini_align (2);
yaw_e (1) = imu.ini_align (3);
% yawm_e (1) = imu.ini_align (3);
DCMnb = euler2dcm ([roll_e (1);pitch_e (1);yaw_e (1);]);
DCMbn = DCMnb ';
作为= euler2qua ([roll_e (1) pitch_e (1) yaw_e (1)]);
%的速度
李vel_e = 0 (3);
%初始化估计在INS时间= 1
vel_e (1) = gnss.vel (1:);
%的位置
李lat_e = 0 (1);
李lon_e = 0 (1);
李h_e = 0 (1);
%初始化估计在INS时间= 1
h_e (1) = gnss.h (1);
lat_e (1) = gnss.lat (1);
lon_e (1) = gnss.lon (1);
%的偏见
gb_dyn = imu.gb_dyn ';
ab_dyn = imu.ab_dyn ';
%初始化卡尔曼滤波器矩阵
%之前估计
kf。ξ= [0 (9),imu。gb_dyn imu。ab_dyn]”;%错误矢量状态
kf。π=诊断接头(imu。在i_align_err, gnss.stdv, gnss.std, imu.gb_dyn, imu.ab_dyn].^2);
kf。R =诊断接头([gnss。stdv gnss。stdm].^2);
kf。Q =诊断接头(imu。arw imu。vrw imu。gb_psd, imu.ab_psd]。^ 2);
fb_corrected = (imu.fb (: 1) + ab_dyn);
fn = (DCMbn * fb_corrected);
%向量更新矩阵F
:乌利希期刊指南= [gnss.vel (1) gnss.lat (1) gnss.h (1) fn的];
%更新矩阵F和G
(kf。F, kf。G) = F_update(乌利希期刊指南、DCMbn imu);
(RM, RN) =半径(gnss.lat (1));
Tpr =诊断接头(((RM + gnss.h (1)), (RN + gnss.h (1)) * cos (gnss.lat (1)), 1]);% radians-to-meters
%更新矩阵H
kf。H = [O I O O O;
O O Tpr O O;];
kf。R =诊断接头([gnss。stdv gnss.stdm])。^ 2;
kf。(gnss z =。stdv gnss。澳门旅游娱乐有限公司]”;
%传播之前估计得到xp(1)和页(1)
kf = kf_update (kf);
%卡尔曼滤波器矩阵为以后性能分析
ξ= 0 (LG、15);%的卡尔曼滤波器的状态,
xp = 0 (LG、15);%的卡尔曼滤波器后验,xp
z = 0 (LG、6);% INS / GNSS测量
v = 0 (LG、6);%卡尔曼滤波器的创新
b = 0 (LG、6);%偏见compensantions卡尔曼滤波器后修正
一个= 0 (LG, 225);%过渡态矩阵,
π= 0 (LG, 225);%先验协方差矩阵,π
页= 0 (LG, 225);%后面的协方差矩阵,页
K = 0 (LG, 90);%卡尔曼增益矩阵,K
S = 0 (LG、36);%创新矩阵,
%初始化矩阵为卡尔曼滤波器性能分析
xp (1:) = kf.xp ';
页(1:)=重塑(kf)。页,225);
:b (1) = (imu。gb_sta imu.ab_sta);
% INS (IMU)时间是主时钟
因为我= 2:李
% %惯性导航系统(INS)
% %每10000 INS处决在控制台打印一个点
%如果(mod(我10000)= = 0),流('。”);结束
% %每200000 INS执行打印控制台回报
%如果(mod(我200000)= = 0),流(“\ n”);结束
% IMU采样间隔
dti = imu.t (i) - imu.t(张);
%惯性传感器与KF纠正偏差估计
wb_corrected = (imu.wb(我:)+ gb_dyn);
fb_corrected = (imu.fb(我:)+ ab_dyn);
% Turn-rates更新
omega_ie_n = earthrate (lat_e(张));
omega_en_n = transportrate (lat_e(张),vel_e(张,1),vel_e(张,2),h_e(张));
%的态度更新
[qua_n DCMbn,欧拉]= att_update (wb_corrected, DCMbn,必要,…
omega_ie_n omega_en_n, dti、att_mode);
roll_e (i) =欧拉(1);
pitch_e (i) =欧拉(2);
yaw_e (i) =欧拉(3);
作为= qua_n;
%重力更新
gn =重力(lat_e(张),h_e(张));
%的速度更新
fn = (DCMbn * fb_corrected);
vel_n = vel_update (fn vel_e(张:),omega_ie_n, omega_en_n, gn的dti);
:vel_e(我)= vel_n;
%位置更新
pos = pos_update ([lat_e(张)lon_e(张)h_e(张)],vel_e(我,:),dti);
lat_e (i) = pos (1);
lon_e (i) = pos (2);
h_e (i) = pos (3);
% ZUPT检测算法
idz =地板(gnss。zupt_win / dti);%索引设置ZUPT窗口时间
如果(我> idz)
vel_m =意味着(vel_e (i-idz:我:));
如果(abs (vel_m) < = gnss.zupt_th)
roll_e (i) =意味着(roll_e (i-idz:我:));
pitch_e (i) =意味着(pitch_e (i-idz:我:));
yaw_e (i) =意味着(yaw_e (i-idz:我:));
lat_e (i) =意味着(lat_e (i-idz:我:));
lon_e (i) =意味着(lon_e (i-idz:我:));
h_e (i) =意味着(h_e (i-idz:我:));
zupt = true;
结束
结束
% %卡尔曼滤波更新
%检查如果有新的GNSS数据在当前INS时间过程
gdx = (gnss找到。t > = (imu.t (i) - gnss.eps) & gnss。t < (imu.t(我)+ gnss.eps));
如果股票指数型基金(~ isempty股票指数型基金(简称eft)(& &简称eft > (1)
% %的创新
(RM, RN) =半径(lat_e(我));
Tpr =诊断接头(((RM + h_e(我),(RN + h_e(我))* cos (lat_e(我)),1]);% radians-to-meters
%的创新与杠杆臂位置修正
zp = Tpr * ([lat_e(我);lon_e(我);h_e (i);] - [gnss.lat股票指数型基金(简称eft);gnss.lon股票指数型基金(简称eft);gnss.h股票指数型基金(简称eft);])……
+ (DCMbn * gnss.larm);
%的创新与杠杆臂速度修正
zv = (vel_e(我:)——gnss.vel (gdx:) - ((omega_ie_n + omega_en_n)。* (DCMbn * gnss.larm))”……
+ (DCMbn * skewm * gnss (wb_corrected)。larm)“)”;
% %卡尔曼滤波器
% GNSS采样间隔
壳体= gnss.t股票指数型基金(简称eft)(纽——gnss.t (gdx-1);
%向量更新矩阵F
乌利希期刊指南= [vel_e(我,:)lat_e (i) h_e(我)fn ');
%更新矩阵F和G
(kf。F, kf。G) = F_update(乌利希期刊指南、DCMbn imu);
%更新矩阵H
如果(zupt = = false)
kf。H = [O I O O O;
O O Tpr O O;];
kf。R =诊断接头([gnss。stdv gnss.stdm])。^ 2;
kf。z = [zv ' zp '] ';
其他的
kf。H = [O I O O O;];
kf。R =诊断接头([gnss.stdv])。^ 2;
kf。z = zv;
结束
%执行扩展卡尔曼滤波器
kf.xp (1:9) = 0;%州1:9被迫为零(误差状态方法)
kf =卡尔曼滤波(kf,壳体;
% % INS / GNSS修正
%四元数修正
% Crassidis。7.34和A.174a情商。
antm = [0 qua_n (3) -qua_n (2);-qua_n (3) 0 qua_n (1);qua_n (2) -qua_n (1) 0);
作为= qua_n + 0.5。* [qua_n眼睛(4)* (3)+ antm;1 . * (qua_n (1) qua_n (2) qua_n (3)]] * kf.xp (1:3);
作为=必要/规范(必要);%的强力正常化
% DCM校正
DCMbn = qua2dcm(必要);
% %调整另一个可能的态度
%欧拉= qua2euler(必要);
% roll_e (i) =欧拉(1);
% pitch_e (i) =欧拉(2);
% yaw_e (i) =欧拉(3);
%
% E = skewm (kf.xp (1:3));
% DCMbn =(眼(3)+ E) * DCMbn_n;
%的态度纠正
roll_e (i) = roll_e (i) - kf.xp (1);
pitch_e (i) = pitch_e (i) - kf.xp (2);
yaw_e (i) = yaw_e (i) - kf.xp (3);
%的速度修正
vel_e (1) = vel_e(我,1)- kf.xp (4);
vel_e(我,2)= vel_e(我,2)——kf.xp (5);
vel_e (3) = vel_e(我,3)——kf.xp (6);
%的位置修正
lat_e (i) = lat_e (i) - (kf.xp (7));
lon_e (i) = lon_e (i) - (kf.xp (8));
h_e (i) = h_e (i) - kf.xp (9);
%偏差修正
gb_dyn = kf.xp(十12);
ab_dyn = kf.xp(行);
%矩阵为以后卡尔曼滤波器性能分析
股票指数型基金xi(简称eft:) = kf.xi ';
股票指数型基金xp(简称eft:) = kf.xp ';
股票指数型基金(简称eft:) = (gb_dyn, ab_dyn];
股票指数型基金(简称eft:) =重塑(kf)。一,225);
股票指数型基金π(简称eft:) =重塑(kf)。π,225);
股票指数型基金(简称eft:) =重塑(kf)。页,225);
如果(zupt = = false)
:股票指数型基金(简称eft) = kf.v ';
股票指数型基金K(简称eft:) =重塑(kf)。K, 90);
股票指数型基金(简称eft:) =重塑(kf)。年代,1、36);
其他的
zupt = false;
股票指数型基金(简称eft:) = (kf。v ' 0 0 0) ';
股票指数型基金K(简称eft 1:45) =重塑(kf)。45 K 1);
股票指数型基金(简称eft书1:9)=重塑(kf)。年代,1、9);
结束
结束
结束
% %总结从INS / GNSS集成
nav_e。t = imu。t(1:我:);% INS时间向量
nav_e。tg = gnss.t;% GNSS时间向量,向量的时间时,卡尔曼滤波器被处决
nav_e。滚= roll_e(1:我:);%卷
nav_e。距= pitch_e(1:我:);%球场
nav_e。偏航= yaw_e(1:我:);%偏航
nav_e。韦尔= vel_e(1:我:);% NED速度
nav_e。lat = lat_e(1:我:);%纬度
nav_e。朗= lon_e(1:我:);%经度
nav_e。h = h_e(1:我:);%的高度
nav_e。ξ= xi;%的国家
nav_e。xp = xp;%后验状态
nav_e。m = z;% IMU / GPS测量
nav_e。v = v;%卡尔曼滤波器的创新
nav_e。b = b;%偏差补偿
nav_e。一个=;%转换矩阵
nav_e。π=π;%先验协方差矩阵
nav_e。页=页;%后验协方差矩阵
nav_e。K = K;%卡尔曼增益矩阵
nav_e。=年代;%创新矩阵
流(' \ n ');
结束
dt函数kf =卡尔曼滤波(kf)
%卡尔曼:卡尔曼滤波算法。
%
%的输入
% kf:数据结构至少有以下字段:
% xp: 21 x1后验状态向量(旧)。
% z: 6 x1创新向量。
% F: 21 x21状态转换矩阵。
% H: 6 x21观测矩阵。
%问:12 x12过程噪声协方差。
% R: 6 x6观测噪声协方差。
%页:21 x21后验误差协方差。
% G: 21 x12控制输入矩阵。
% dt:采样间隔。
%
%输出
% kf:更新以下字段:
% xi: 21 x1先验状态向量(更新)。
% xp: 21 x1后验状态向量(更新)。
% v: 6 x1创新向量。
%:21 x21状态转换矩阵。
% K: 21 x6卡尔曼增益矩阵。
% Qd: 21 x6离散过程噪声协方差。
%π:21 x21先验误差协方差。
%页:21 x21后验误差协方差。
% S: 6 x6创新协方差(或剩余)。
%
%的预测步骤
kf = kf_prediction (kf, dt);
%更新步骤
kf = kf_update (kf);
结束
函数kf = kf_update (kf)
%卡尔曼:更新测量卡尔曼滤波算法的一部分。
%
%的输入
% kf:数据结构至少有以下字段:
% xi: 15 x1先验状态向量。
%π:15 x15先验误差协方差。
% z: 6 x1创新向量。
% H: 6 x15观测矩阵。
% R: 6 x6观测噪声协方差。
%
%输出
% kf:更新以下字段:
% xp: 15连接后验状态向量(更新)。
%页:15连接后验误差协方差(更新)。
% v: 6 x1创新向量。
% K: 15 x6卡尔曼增益矩阵。
% S: 6 x6创新协方差(或剩余)。
% I =眼睛(大小(kf.F));
%找到卡尔曼增益通过v和我。e创新
kf。S = (kf)。R + kf。H * kf。π* kf.H’);%创新(或pre-fit残留)协方差矩阵
kf。v = kf。z - kf。H * kf。xi; % Innovations or measurement pre-fit vector
kf。K = (kf)。π* kf.H”) * (kf.S) ^ (1);%卡尔曼增益矩阵
%更新后验状态估计(xp)
kf。xp = kf。习+ kf。K * kf。v;
%更新后验估计协方差矩阵(Pp)
kf。页= kf。π- kf。K * kf。* kf.K ';
kf。页= 0.5 * (kf。Pp + kf.Pp ');%迫使π是对称矩阵
结束
