[Filter tracking] based on matlab unscented Kalman filter inertial navigation + DVL combined navigation [including Matlab source code 2019]

一、Autonomous navigation technology introduction

1 基于SINS/声学的AUV自主导航
1.1 基于SINS/DVL的AUV自主导航技术
DVLIs based on the sonar doppler speed measuring equipment, Can provide higher accuracy of vehicle speed information, And the error does not accumulate over time, 抗干扰能力强.因此在AUVAutonomous navigation in theDVLCan be used as inhibition ofSINSAccumulated error of important supplementary means.DVLSpeed configuration is divided into3种:Single beam configuration、双波束JanusConfiguration and four beamJanus配置, The configuration of speed measuring principle as shown in figure1所示.

图1 3Doppler velocity log log configuration diagram
由图中可以看出, Using four beamJanus配置方式的DVLTo determine the horizontal and vertical velocity vector information, 更能满足AUVThe navigation requirements.SINS/DVLIntegrated navigation system are mainly pine、紧2种组合方式.原理示意图如图2所示.DVLThe raw data is measuring the relative speed in the direction of the beam, SINS/DVLLoose combination and close combination of theory and its characteristics such as table1所示[7,8].

图2 捷联惯性导航系统/Doppler velocity log combination principle diagram
SINS/DVLThe key technology of integrated navigation system involved mainly have the following5个方面:数据融合技术、The asynchronous information fusion technology、标定技术、DVLFailure data processing and fault detection technology.

1. 数据融合技术

SINS/DVLIntegrated navigation system for data fusion using navigation filter.Kalman filter is an efficient optimal estimation algorithm, 自20世纪70S has been widely used in integrated navigation system, Danish productionMARIDAN150型AUV[7]And Norway productionHUGIN 1000型AUV[16]Using the kalman filter.针对DVLSpeed is influenced by the complex underwater environment, 文献[17]将改进后的Sage-HusaAdaptive kalman filter algorithm is introducedSINS/DVLIn the integrated navigation system, 仿真结果表明, This algorithm has high navigation accuracy and robustness.针对SINS/DVLIntegrated navigation system of the state of nonlinear model, 文献[18]利用5Order of spherical the minimalist phase diameter volume kalman filter algorithm to improve theSINS/DVLIntegrated navigation accuracy.

The kalman filter assumes that the system noise and measurement noise obey gaussian probability density function of, 但在实际工程应用中, Often difficult to meet the above conditions, SINS/DVLNoise may present the measurement of the integrated navigation of the thick tail non-gaussian distribution.因此, Through robust kalman filter algorithm to improve the precision of integrated navigation system in complex environment is important and urgent[19].文献[20]Based on the studenttDistribution of noise modeling robust gaussian approximation filters to state vector and the unknown distribution parameters are estimated joint, 在理论上, The filter can eliminate complex underwater environment forSINS/DVLThe effects of integrated navigation, 以提高SINS/DVLIntegrated navigation accuracy.

2. The asynchronous information fusion technology

实际应用中, 由于SINS和DVL采样频率不同, And the data fusion center receives theSINSThe navigation information andDVLThe speed of the information may not exist at the same time long lag, We need to study the asynchronous multisensor information fusion algorithm.For multi-sensor network system of asynchronous state fusion estimation problem, 文献[21]Derived the form of recursive linear minimum mean square error estimator is, The estimator is collect by data fusion time interval of the asynchronous measurement value is converted to a combination of equivalent measurement fusion time, Thus it is concluded that the most excellent step estimation fusion algorithm.文献[22]Using the matrix of the linear minimum variance sense weighted fusion estimation algorithm, Presents a distributed optimal fusion estimation, Simulation examples verify the effectiveness of the algorithm.文献[23]In view of the integrated navigation system sensor sampling frequency and different measurement lag problem, Put forward a kind of integrated navigation based on multi-scale data block asynchronous information fusion algorithm, The simulation results show that the algorithm can effectively improveSINS/DVLThe positioning precision of integrated navigation.

3. 标定技术

DVLError mainly includes the misalignment Angle and scale factor error, etc.关于DVLError calibration problem has a lot of research, The earliest only for heading misalignment Angle error calibration.随着研究的深入, Completed the three axis misalignment Angle error and other errors calibration.Misalignment Angle calibration problem is usually transformed into the estimation problem of the transformation matrix between two points set, These methods require a external auxiliary sensor can provide accurate positioning information.Estimation technology including the least squares estimation、求解Wahba’s问题、Rigid body rotation group of adaptive identification, etc.其中:文献[24]利用INS/GPSNavigation solution of integral andDVLMeasured value is constructed2个点集, The method based on singular value decomposition least squares estimation method forWahbar’sThe most stable solutions, And completed the misalignment Angle error and scale factor error compensation;文献[25]在DVLBased on the principle of speed, Analyzes the error source, Based on the scaling factor is established andINS与DVLThe misalignment Angle error model, And velocity error as the observed value design kalman filter, The observability analysis method to estimate the misalignment Angle and scale factor, And in setting3Sport cases observed analysis was carried out on the calibration parameters, Finally the reliability of the algorithm was verified by means of comprehensive testing, Compared with the traditional calibration method, The calibration method shortens the time and calibration distance;文献[26]Based on the point of observability analysis of the same part under a certain trajectory misalignment Angle calibration not reason, 并提出了一种INS和DVLThree online calibration method, The method by underwater vehicle float on the water2Time to receive the global navigation satellite system (global navigation satellite system, GNSS) Signal to improve the navigation system observability, Finally complete the misalignment Angle and scale factor calibration;文献[27]From the Angle of the control system is studiedIMU/DVLCombination of underwater navigation, Analysis shows that under the condition of the medium movement combination system is observable, 因此DVLMisalignment Angle and scale factor error can not rely on additional externalGPSUnder the condition of acoustic beacon or on-site calibration.同时在DVLAided by measuring, Also can effectively estimateIMUThe bias error.与上述方法相比, 文献[28]充分利用了INSNavigation parameters the method using the velocity and acceleration completed forDVLAnd attitude sensor alignment between the rotation matrix online calibration, To eliminate the dependence of the external input conditions, Its main advantage is not need to rely on additional external navigation information, Therefore is suitable for the long navigation of underwater navigation.

4. DVLFailure data processing

DVLNeeds to receive the reflected beam, Its receiving acoustic signal with the acoustic environment has much to do.DVLWorking in the bottom tracking mode, In Marine creatures block、Bottom absorption geological、Deep groove and the carrier under the large Angle motion, and so on and so forth will appearDVL数据失效, This directly affects its navigation performance.解决DVLMethods mainly include data failure2种:A method for the isolation, The direct isolation offDVLFailure data;Another is replacing method, The replaceDVLThe speed of data.Isolation method of nature is toSINS/DVLIntegrated navigation system become pure inertial navigation system, Navigation accuracy decreases.The present study hotspot for replacing method, This method is to design a vehicle relative to the speed of the estimator is to replaceDVLSpeed information.文献[29]在DVL数据失效时, 利用DVLWater tracking measurement asDVL速度量测, 为SINSProvide speed buffs.文献[30]Based on a partial least-squares regression (partial leas squares regression, PLSR) 和支持向量回归 (suppor vector regression, SVR) Method of combining the establishedPLSR-SVR预测器, 有效延长DVLFailure data fault-tolerant time so as to improve the navigation precision and reliability of the.文献[31]Online estimate the current parameters of navigation task model is put forward, The average speed of current embedded real-time estimation, Using the model of auxiliary autonomy and improve system robustness.文献[32]Combined with the real time current estimate, Model aided inertial navigation system was put forward, 在DVL数据失效时, To improve the autonomy and robustness of navigation system.

5. 故障检测技术

For the acoustic scattering、Fishery group and the bottom of the sea gullyDVLMutations in the noise produces the horizontal posture error, And the position error accumulation for the question, 文献[33]提出了一种基于2Rules of fault diagnosis methods, When noise mutation, Used in the model update speed time for data fusion, 而不是用DVLThe speed of data fusion.研究SINS/DVLIntegrated navigation system failure detection method, To improve the reliability of integrated navigation system has great significance to.文献[34]By using the wavelet technology for fault diagnosis of sensor output signal, Fault isolation and system reconfiguration and, The information fusion navigation parameter by federated filter.The most widely used in integrated navigation system failure detection method is the chi-square test.文献[35]This paper proposes a multi-sensor redundancy navigation system failure detection algorithm, The algorithm based on sequential probability ratio test (sequential probability ratio test, SPRT) And chi-square test to detect fault levels andSPRTMonitoring the fault trend, Comprehensive diagnosis of fault.

SINS/DVLIntegrated navigation method isAUVThe current mainstream use underwater autonomous navigation technology.Although on the key technology of the obtained bigger achievements, 但DVLCan only output speed information, Rather than output location information, 进而SINS/DVLThe location of the integrated navigation system cannot be observed, So as to make the position spread over time still.相比于DVL, Underwater acoustic positioning technology or geophysical navigation technology can provideAUV位置信息, 因此, Can use the above2Kind of technology to further improve the navigation precision.

1.2 基于SINS/Underwater acoustic positioningAUV自主导航技术
Relative to the electromagnetic wave, The spread of sound waves in sea water attenuation effect is much smaller.因此, Underwater acoustic positioning technology inAUVAutonomous navigation plays an important role in.Underwater acoustic positioning system according to the classification of baseline length can be divided intoLBL、SBL和USBL 3种.LBLThe baseline length can be compared to the deep sea to, Array is composed of multiple distribution on the sea bed transponder, 定位精度高, Suitable for use in the area of the large-scale operations;But the data update rate is low, The transponder cloth、Calibration and recycling、Maintain all red, 作业成本高[36].SBLThe baseline length is commonly a few meters to tens of meters between, The primitive distribution on the bottom or side.Restricted by the baseline length, SBLThe accuracy betweenLBL和USBL之间, And the tracking range smaller, More suitable for near the mother shipAUV导航定位.USBLThe array can be integrated within a compact unit, Baseline length of decimeter level and less than or equal to a half wavelength, Its minimum volume size, Can be conveniently placed in flow and structure noise are weaker a vantage point, And the cloth、Recycling is very convenient, 因此, USBLBy more and more widely attention and application.但USBL的精度低于LBL和SBL, Sensor and location accuracy relies heavily on depth、Position sensor and other peripheral equipment, 如何提高USBLThe positioning accuracy of became a hot issue of research in this field.3Kind of underwater acoustic positioning system diagram as shown in figure3, 性能对比如表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] An an,Yong-gang zhang,赵玉新.Autonomous underwater vehicle navigation methods reviewed[J].Journal of underwater unmanned systems. 2019,27(03)

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