【滤波跟踪】无迹卡尔曼滤波惯性导航+DVL组合导航【含Matlab源码 2019期】

?博主简介：热爱科研的Matlab仿真开发者，修心和技术同步精进，Matlab项目合作可私信。
🍎个人主页：海神之光
🏆代码获取方式：
海神之光Matlab王者学习之路—代码获取方式
??座右铭：行百里者，半于九十。

更多Matlab仿真内容点击👇
Matlab图像处理（进阶版）
路径规划（Matlab）
神经网络预测与分类（Matlab）
优化求解（Matlab）
语音处理（Matlab）
信号处理（Matlab）
车间调度（Matlab）

?一、自主导航技术简介

1 基于SINS/声学的AUV自主导航
1.1 基于SINS/DVL的AUV自主导航技术
DVL是基于声呐多普勒效应的测速设备, 能提供较高精度的载体速度信息, 且其误差不随时间积累, 抗干扰能力强。因此在AUV自主导航中DVL可作为抑制SINS积累误差的重要辅助手段。DVL测速的配置分为3种:单波束配置、双波束Janus配置和四波束Janus配置, 其配置的测速原理如图1所示。

图1 3种多普勒速度计程仪测速配置示意图
由图中可以看出, 采用四波束Janus配置方式的DVL能够同时测定载体的横向和纵向的速度信息, 更能满足AUV的导航需求。SINS/DVL组合导航系统主要有松、紧2种组合方式。原理示意图如图2所示。DVL的原始数据是每个波束方向测量的相对速度, SINS/DVL松组合和紧组合的原理及其特点如表1所示[7,8]。

图2 捷联惯性导航系统/多普勒速度计程仪组合原理示意图
SINS/DVL组合导航系统所涉及的关键技术主要有以下5个方面:数据融合技术、异步信息融合技术、标定技术、DVL数据失效处理和故障检测技术。

1. 数据融合技术

SINS/DVL组合导航系统使用导航滤波器进行数据融合。卡尔曼滤波器是一种有效的最优估计算法, 自20世纪70年代以来已广泛应用于组合导航系统, 丹麦生产的MARIDAN150型AUV[7]和挪威生产的HUGIN 1000型AUV[16]均使用卡尔曼滤波器。针对DVL测速易受到复杂的水下环境影响, 文献[17]将改进后的Sage-Husa自适应卡尔曼滤波算法引入SINS/DVL组合导航系统中, 仿真结果表明, 该算法具有较高的导航精度和鲁棒性。针对SINS/DVL组合导航系统的非线性状态模型, 文献[18]利用5阶球面最简相径容积卡尔曼滤波算法来改善SINS/DVL组合导航精度。

卡尔曼滤波器假设系统噪声和量测噪声的概率密度函数服从高斯分布, 但在实际工程应用中, 常难以满足上述条件, SINS/DVL组合导航的量测噪声可能呈现厚尾非高斯分布。因此, 通过鲁棒卡尔曼滤波算法来改善组合导航系统在复杂环境下的精度是重要且紧迫的[19]。文献[20]提出基于学生t分布噪声建模的鲁棒高斯近似滤波器来对状态向量和未知分布参数进行联合估计, 在理论上, 该滤波器能消除水下复杂环境给SINS/DVL组合导航带来的影响, 以提高SINS/DVL组合导航精度。

2. 异步信息融合技术

实际应用中, 由于SINS和DVL采样频率不同, 而且数据融合中心接收到的SINS的导航信息与DVL的速度信息可能存在不同时长的滞后, 故需要研究异步多传感器的信息融合算法。针对多传感器网络系统的异步状态融合估计问题, 文献[21]推导了递归形式的线性最小均方误差估计器, 该估计器将数据融合时间段所间隔收集的异步量测值转换为融合时刻组合等效的量测值, 从而得出最优异步估计融合算法。文献[22]利用线性最小方差意义下的矩阵加权融合估计算法, 给出了分布式最优融合估计, 仿真实例验证了算法的有效性。文献[23]针对组合导航系统的传感器采样频率不同且存在量测滞后的问题, 提出一种基于多尺度数据分块的组合导航异步信息融合算法, 仿真结果表明该算法可有效提高SINS/DVL组合导航的定位精度。

3. 标定技术

DVL误差主要包括了失准角误差及比例因子误差等。关于DVL误差标定问题已有许多研究, 最早的标定只针对航向失准角误差标定。随着研究的深入, 完成了对三轴失准角误差及其他误差的标定。失准角标定问题通常转化为两点集之间变换矩阵的估计问题, 这些方法需要一个可以提供准确定位信息的外部辅助传感器。估计技术包括最小二乘估计、求解Wahba’s问题、刚体旋转组的自适应辨识等。其中:文献[24]利用INS/GPS积分的导航解和DVL测量值构造了2个点集, 该方法基于奇异值分解的最小二乘估计方法作为Wahbar’s问题最稳定的解, 并完成了对失准角误差及比例因子误差的补偿;文献[25]在DVL测速原理的基础上, 分析了误差来源, 建立了基于比例因子及INS与DVL失准角的误差模型, 并以速度误差为观测值设计卡尔曼滤波器, 采用可观测性分析的方法对失准角及比例因子进行估计, 并在设定的3种运动情况下对标定参数进行可观测分析, 最后通过综合试验验证了算法的可靠性, 与传统标定方法相比, 该标定方法缩短了标定时间和标定距离;文献[26]同样基于可观测性的观点分析了在某些运动轨迹下部分失准角不可标定的原因, 并提出了一种INS和DVL的三点在线标定方法, 该方法通过水下航行器浮上水面2次接收到全球导航卫星系统 (global navigation satellite system, GNSS) 信号去改善导航系统的可观测性, 最后完成失准角及比例因子的标定;文献[27]从控制系统的角度研究了IMU/DVL组合水下导航, 分析表明在中等运动条件下组合系统是可观测的, 因此DVL失准角及比例尺因子误差可以在不依赖额外的外部GPS或声学信标的情况下进行现场校准。同时在DVL测量的辅助下, 还可以有效估计IMU的偏置误差。与上述方法相比, 文献[28]充分利用了INS导航参数该方法利用速度与加速度参数完成了对DVL与姿态传感器之间的对准旋转矩阵在线标定, 消除了对外部输入条件的依赖性, 其主要优点是不需要依靠额外的外部导航信息, 因此适用于水下航行的长时导航。

4. DVL数据失效处理

DVL需要接收外界的反射波束, 其接收的声学信号与周围声学环境有很大关系。DVL在底跟踪工作模式下, 遇到海洋生物阻挡、海底强吸声地质、深沟和载体大角度运动等情况下会出现DVL数据失效, 这直接影响其导航性能。解决DVL数据失效的方法主要有2种:一种为隔离法, 即直接隔离掉DVL失效的数据;另一种为替换法, 即替换掉DVL的测速数据。隔离法本质是将SINS/DVL组合导航系统变成纯惯性导航系统, 导航精度会降低。目前研究热点为替换法, 该方法是设计一个载体相对地的速度估计器来替换掉DVL测速信息。文献[29]在DVL数据失效时, 利用DVL水跟踪量测作为DVL速度量测, 为SINS提供速度辅助。文献[30]基于一种偏最小二乘回归 (partial leas squares regression, PLSR) 和支持向量回归 (suppor vector regression, SVR) 相结合的方法建立了PLSR-SVR预测器, 有效延长DVL数据失效的容错时间从而提高了导航精度和可靠性。文献[31]提出了在线估计导航任务的海流参数模型, 其对海流的平均速度进行嵌入式实时估计, 利用模型辅助有效提高系统自主性和鲁棒性。文献[32]结合实时海流估计, 提出了模型辅助惯性导航系统, 在DVL数据失效时, 提高了导航系统的自主性和鲁棒性。

5. 故障检测技术

针对由声散射、渔群和海底冲沟引起的DVL中的突变噪声会产生水平姿态误差, 并累积为位置误差的问题, 文献[33]提出了一种基于?2规则的故障诊断方法, 当噪声发生突变时, 采用模型中的速度时间更新来进行数据融合, 而不是用DVL中的速度进行数据融合。研究SINS/DVL组合导航系统故障检测方法, 对提高组合导航系统的可靠性有重大意义。文献[34]利用小波技术对传感器输出信号进行故障诊断, 并进行故障隔离和系统重构, 再采用联邦式滤波器进行信息融合得出导航参数。组合导航系统中应用最广泛的故障检测方法即为卡方检测法。文献[35]提出了一种多传感器冗余导航系统的故障检测算法, 该算法根据序列概率比检验 (sequential probability ratio test, SPRT) 和卡方检验监测到的故障等级以及SPRT监测到的故障趋势, 对故障进行综合诊断。

SINS/DVL组合导航方法是AUV当前主流使用的水下自主导航技术。尽管在上述的关键技术上获得了较大成果, 但DVL只能输出速度信息, 而不能输出位置信息, 进而SINS/DVL组合导航系统的位置不可观测, 从而使得位置随着时间推移仍然发散。相比于DVL, 水声定位技术或地球物理辅助导航技术可以提供AUV位置信息, 因此, 可利用以上2种技术进一步提高导航精度。

1.2 基于SINS/水声定位的AUV自主导航技术
相对于电磁波而言, 声波在海水中传播的衰减效应要小的多。因此, 水声定位技术在AUV自主导航中扮演着重要的角色。水声定位系统按基线长度分类可分为LBL、SBL和USBL 3种。LBL的基线长度可与海深相比拟, 基阵由多个分布于海床上的应答器组成, 定位精度高, 适合在大面积作业区域内使用;但其数据更新率较低, 应答器的布放、校准以及回收、维护都异常繁琐, 作业成本高[36]。SBL的基线长度一般为几米到几十米之间, 各基元分布在船底或船舷上。受基线长度限制, SBL的精度介于LBL和USBL之间, 且其跟踪范围较小, 更适合于在母船附近的AUV导航定位。USBL的基阵可以集成于一个紧凑的整体单元内, 基线长度为分米级或小于等于半波长, 其体积尺寸最小, 可方便地安置于流噪声和结构噪声均较弱的某个有利位置, 且布放、回收极为便捷, 因此, USBL受到了越来越广泛的关注和应用。但USBL的精度低于LBL和SBL, 且定位精度非常依赖于深度传感器、姿态传感器等外围设备, 如何提高USBL的定位精度成为该领域研究的热点问题。3种水声定位系统的示意图见图3, 性能对比如表3所示。

?二、部分源代码

%% Set initial conditions
clear;
clc;
close all;
T=3000; %Total simulation time
INS_T=0.01;

DVL_T=1;
DVL_S=0;

P_filter=[1 2 3];
clrs = {‘b’,‘r’,‘g’,‘m’,‘b-.’};
legs={‘ESKF’,‘UKF’,‘EKF’,‘ESKF1’};
legs3sig={‘SAM 3\sigma’,‘PA 3\sigma’,‘SA 3\sigma’};
defaultfontname=‘Times New Roman’;
defaultfontsize=15;
defaultfontweight=‘normal’;
set(0,‘defaultlinelinewidth’,4)

% DVL_S=10.001;

N_INS=T/INS_T;
N_DVL=fix((T-DVL_S)/DVL_T);

% Constants
R0=6378137;
f=(6378137-6356752.3142)/6378137;
e=sqrt(f*(2-f)) ; %Earth eccentric
% grav0=9.780318*(1+5.302410^-3*sin(pi/6)2-5.910^{-6*sin(2*pi/6)}2);
dph2rps = (pi/180)/3600; % conversion constant from deg/hr to rad/sec
deg_to_rad = 0.01745329252;
rad_to_deg = 1/deg_to_rad;
micro_g_to_meters_per_second_squared = 9.80665E-6;

%% Generating Simulation data

Measurement_Noise.DVL=1*[2 2 2]‘* 1E-3; %in m/s measure vg
Measurement_Noise.DVL_FACTOR=[1 1 1]’* 1E-2; %in m

Measurement_Noise.AHRS=1*[0.03 0.03 0.3]'; %in degree
Measurement_Noise.Z=1*10^(-3);

Measurement_Noise.ACC=18 …
micro_g_to_meters_per_second_squared[1 ;1;1]; %in m/s measure vg
Measurement_Noise.ACC_BIAS=1[50 50 50]'micro_g_to_meters_per_second_squared;
% Measurement_Noise.ACC_BIAS_SIG=1[1;1;1]; %standrad derivation of bias of GYO, rad/s
Measurement_Noise.ACC_FACTOR=1*[5, 0, 0;…
0, 5, 0;…
0, 0, 5] * 1E-4; % XX%
% Measurement_Noise.ACC_QUANT=1E-1*[1 ;1;1];

Measurement_Noise.GYO=1* 0.02*[1 ;1;1]60 dph2rps ; %random walk of GYO, rad/s
Measurement_Noise.GYO_BIAS=0.1*[1 1 1]’ * dph2rps; %bias of GYO, rad/s
% Measurement_Noise.GYO_BIAS_SIG=10.1dph2rps*[1;1;1]; %standrad derivation of bias of GYO, rad/s
Measurement_Noise.GYO_FACTOR=1* [5, 0, 0;…
0, 5, 0;…
0, 0, 5] * 1E-4;
% Measurement_Noise.GYO_QUANT=2E-3*[1 ;1;1];

% Navigation frame: NED
Target0.p0=[30pi/180 30pi/180 10 0deg_to_rad 0deg_to_rad 0*deg_to_rad]‘; %Target initial position
Target0.v0=[1.2 0 0 0 0 0]’; %Target initial velocity
Target0.vcurrent=[0 0 0]'; %Constant Irrotational current

Time.T=T; %Total simulation time
Time.INS_T=INS_T;
Time.DVL_T=DVL_T;
% Time.INS_S=10.1;
Time.DVL_S=DVL_S;

[p,pCa, VnS, AttS, AccS,US, ACCm, GYOm, DVLm,AHRSm,Zm] = Simulation_DVL_INS(Time,Target0,Measurement_Noise);

%
% IMUm=[ACCm(1:3,:);GYOm]‘;
% DVLm=DVLm’;
% Real_data=[p(1:6,:);pCa(1:3,:);VnS(1:3,:);AttS(1:3,:);AccS(1:3,:);US]‘;
%
%
% fid=fopen(‘DVL.txt’,‘wt’); %写入文件路径
% [m,n]=size(DVLm);
% for i=1:1:m
% for j=1:1:n
% if jn
% fprintf(fid,‘%f\n’,DVLm(i,j));
% else
% fprintf(fid,‘%f\t’,DVLm(i,j));
% end
% end
% end
% fclose(fid);
%
% fid=fopen(‘IMU.txt’,‘wt’); %写入文件路径
% [m,n]=size(IMUm);
% for i=1:1:m
% for j=1:1:n
% if jn
% fprintf(fid,’%f\n’,IMUm(i,j));
% else
% fprintf(fid,‘%f\t’,IMUm(i,j));
% end
% end
% end
% fclose(fid);
%
% fid=fopen(‘Real_data.txt’,‘wt’); %写入文件路径
% [m,n]=size(Real_data);
% for i=1:1:m
% for j=1:1:n
% if j==n
% fprintf(fid,‘%f\n’,Real_data(i,j));
% else
% fprintf(fid,‘%f\t’,Real_data(i,j));
% end
% end
% end
% fclose(fid);

% pStore—LaLo representation, Real AUV postions at DVL/AHRS sampling epoch 6N_INS matrix
% pCaStore—Cartesian representation, Real AUV postions at DVL/AHRS sampling epoch 6N_R matrix
% ACCm—Accelerometer measurements 3N_INS matrix (fx fy fz in m/s^2)
% GYOm—Gyroscope measurements 3N_INS matrix (p q r in degree/s)
% DVLm—DVL measurements 3N_INS matrix (u,v, w in m/s)
% AHRSm—AHRS measurements 3N_INS matrix (phi theta psi in degree)
%Zm—Depth sensor measurements 1*N_INS matrix (Z in m)

%%%%%%%%%%%%%%%%%%%%THE END%%%%%%%%%%%%%%%%%%%

%% Initialized
fprintf(1,’ Starting NAV computations \n’);
% load(‘fltTest.mat’);
p_0=Target0.p0(1:3);
v_0=Target0.v0(1:3);
A_0=Target0.p0(4:6);

q_0 = euler2q(A_0(1), A_0(2), A_0(3));
bg_0=zeros(3,1);
ba_0=zeros(3,1);
Kg_0=zeros(3,1);
Ka_0=zeros(3,1);

% % % % % % % % % % % % ESKF%%%%%%%%%%%%%%%%%%%%%%%%%%%%
x_eskf_0=[q_0; p_0;v_0;bg_0;ba_0;Kg_0;Ka_0];
xe_eskf_0=zeros(21,1);
x_eskf=x_eskf_0;
xe_eskf=xe_eskf_0;

P_att_eskf=3.046210^-4eye(3);
P_pos_eskf=diag([5/R0 5/R0 5]).^2;
P_vel_eskf=0.01eye(3);
P_bg_eskf=610^-11eye(3);
P_ba_eskf=610^-8eye(3);
P_Kg_eskf=0.000051eye(3);
P_Ka_eskf=0.00051*eye(3);

P_0_eskf=blkdiag(P_att_eskf, P_pos_eskf, P_vel_eskf, P_bg_eskf, P_ba_eskf, P_Kg_eskf, P_Ka_eskf);
P_eskf=P_0_eskf;

positionStore_eskf=zeros(3,N_INS-1); positionStore_eskf(:,1)=p_0;
velocityStore_eskf=zeros(3,N_INS-1); velocityStore_eskf(:,1)=v_0;
velocitybStore_eskf=zeros(3,N_INS-1); velocitybStore_eskf(:,1)=DCM(A_0)'*v_0;
UStore_eskf=zeros(1,N_INS-1); UStore_eskf(:,1)=norm(v_0);
attitudeStore_eskf=zeros(3,N_INS-1); attitudeStore_eskf(:,1)=A_0;
positionCaStore_eskf=zeros(3,N_INS-1); positionCaStore_eskf(:,1)=zeros(3,1);
AccStore_eskf=zeros(3,N_INS-1); AccStore_eskf(:,1)=zeros(3,1); %n系下的加速度
KaStore_eskf=zeros(3,N_INS-1); KgStore_eskf=zeros(3,N_INS-1);
baStore_eskf=zeros(3,N_INS-1); bgStore_eskf=zeros(3,N_INS-1);

?三、运行结果

?四、matlab版本及参考文献

1 matlab版本
2014a

2 参考文献
[1] 黄玉龙,张勇刚,赵玉新.自主水下航行器导航方法综述[J].水下无人系统学报. 2019,27(03)

3 备注
简介此部分摘自互联网，仅供参考，若侵权，联系删除

🍅 仿真咨询
1 各类智能优化算法改进及应用
生产调度、经济调度、装配线调度、充电优化、车间调度、发车优化、水库调度、三维装箱、物流选址、货位优化、公交排班优化、充电桩布局优化、车间布局优化、集装箱船配载优化、水泵组合优化、解医疗资源分配优化、设施布局优化、可视域基站和无人机选址优化

2 机器学习和深度学习方面
卷积神经网络（CNN）、LSTM、支持向量机（SVM）、最小二乘支持向量机（LSSVM）、极限学习机（ELM）、核极限学习机（KELM）、BP、RBF、宽度学习、DBN、RF、RBF、DELM、XGBOOST、TCN实现风电预测、光伏预测、电池寿命预测、辐射源识别、交通流预测、负荷预测、股价预测、PM2.5浓度预测、电池健康状态预测、水体光学参数反演、NLOS信号识别、地铁停车精准预测、变压器故障诊断

3 图像处理方面
图像识别、图像分割、图像检测、图像隐藏、图像配准、图像拼接、图像融合、图像增强、图像压缩感知

4 路径规划方面
旅行商问题（TSP）、车辆路径问题（VRP、MVRP、CVRP、VRPTW等）、无人机三维路径规划、无人机协同、无人机编队、机器人路径规划、栅格地图路径规划、多式联运运输问题、车辆协同无人机路径规划、天线线性阵列分布优化、车间布局优化

5 无人机应用方面
无人机路径规划、无人机控制、无人机编队、无人机协同、无人机任务分配

6 无线传感器定位及布局方面
传感器部署优化、通信协议优化、路由优化、目标定位优化、Dv-Hop定位优化、Leach协议优化、WSN覆盖优化、组播优化、RSSI定位优化

7 信号处理方面
信号识别、信号加密、信号去噪、信号增强、雷达信号处理、信号水印嵌入提取、肌电信号、脑电信号、信号配时优化

8 电力系统方面
微电网优化、无功优化、配电网重构、储能配置

9 元胞自动机方面
交通流 人群疏散 病毒扩散 晶体生长

10 雷达方面
卡尔曼滤波跟踪、航迹关联、航迹融合