【滤波跟踪】基于无迹卡尔曼滤波实现惯性导航+DVL的组合导航算法附matlab代码

 1 内容介绍

水下航行器（ＡＵＶ）的产生、发展直至实际使用经过了很长的历程。对于海洋人们从了解认识、开发研究、使用和保护等方面的发展，促使着水下航行器的开发研究工作也逐渐活跃起来。在新世纪中水下航行器还将被重点开发应用。水下航行器研制的重要标准之一就是它的自主性能的评价，也正是这一点一直制约着水下航行器的向前发展。怎样充分利用各种传感器的测量信息来改善提高航行器的定位性能和可依赖程度正是目前研究的关键所在。因为水下环境很复杂，而水下航行器需要保证能进行长时间和大范围的作业能力，因此需要组合系统提高精度较高的速度、位置和姿态角等信息。从目前可以看出捷联惯性导航系统（ＳＩＮＳ）和多普勒计程仪（ＤＶＬ）组合方式的导航系统是在国内外水下航行器的发展中应用较为成熟的捷联式惯性系统的短时定位精度高，具有很强的自主性，但是随着时间延长会产生位置误差的累积的不足。ＳＩＮＳ／ＤＶＬ组合系统正是使用多普勒计程仪测量得到的速度信息的精度较高的特点阻碍了ＳＩＮＳ随时间增长位置误差的不足，具有了较强的自主性和较高的导航精度，在目前的水下导航系统上是采用较多的方式。水下航行器的定位准确程度是决定它能否完成设定任务和安全回来的重要影响因素。在ＡＵＶ上使用捷联式惯性系统有如下的不足：其一是ＳＩＮＳ会跟随时间的延长产生较大误差累积，这点对于需要较长时间在水下工作的ＡＵＶ来说是不允许的；其二是满足精度要求的惯导的成本较高，而对于一些民用低成本的ＡＵＶ来说价格过于昂贵；其三是一些小型的ＡＵＶ装配不了满足精度但体积偏大的ＩＮＳ。所以，从成本和使用空间等多方面考虑，ＩＮＳ并不完全适合安装于一些小型的ＡＵＶ上。对于长时间工作的ＳＩＮＳ／ＤＶＬ组合系统来说，其系统精度过多地是依赖于多普勒，在复杂的水下环境中，比方说当ＡＵＶ下潜深度较小时，ＤＶＬ距离水底太远超出了它的测量范围，又或者说是经过的水域水底是淤泥等强吸波物时，ＤＶＬ测速发射的声波就无法返回来，也就得不到可靠的速度数据；也有可能会发生ＤＶＬ故障测速数据无效，而ＳＩＮＳ的误差会随时间积累，随着ＤＶＬ失效时间延长，将会使系统精度快速下降。因此，在不增加硬件传感器的情况下，我们迫切需要找到一种能保证定位精度，并且满足ＡＵＶ水下导航系统对精度和鲁棒性的要求的导航方法

多传感器数据融合技术在军事和民用领域都得到了广泛的重视，与单传感器相比，多传感器数据融合技术具有如下几个优势：第一是使用几个同类的传感器或者对单一传感器进行多次测量可以得到Ｎ个独立的观测值，把这些观测值进行综合处理能提高对跟踪目标的位置和速度佔计值的精度；第二种方式对观测量的提升是通过多传感器之间的相对运动来实现的。卡尔曼滤波估计的广泛采用始于上世纪６０年代，卡尔曼滤波从本质上说是一种线性递推最优最小方差估计方法，它把来自两个或多个导航传感器的测量信息进行处理，能估计得到各个状态的误差量，用这些估计出来的状态误差量来校正系统的输出，从而达到最优估计的目的。扩展卡尔曼滤波（ＥｘｔｅｎｄｅｄＫａｌｍａｎＦｉｌｔｅｒｉｎｇ，ＥＫＦ）克服了卡尔曼滤波要求观测值是线性的，只适用于线性系统的不足，扩展卡尔曼可以用于对非线性系统的估计。１９７９至１９８７年间，Ｓｐｅｙｅｒ和Ｋｅｒｒ等人先后提出并行分散卡尔曼滤波的思想，解决了集中式卡尔曼滤波处理信息计算量大的问题。后来又出现了同样适用于非线性系统佔计理论的无迹卡尔曼滤波（ＵＫＦ），ＵＫＦ对非线性系统状态误差量的递推和更新是通过设定了一些采样点，对这些采样点进行Ｕ变换［８］的值来近似代替原状态点经过非线性变换后的值。

2 仿真代码

function [x,P]  = CorrectDVL_EKF( ...
    x, ... % previous quaternion states 4x1
    P, ... % previous states (3x1 rotation error, 3x1 velocity, 3x1 gyro bias)
    z) % time since last IMU measurement (sec)

% x=[1 2 3 4 5 6 7 8 9]';
% P=diag(x);
% z=[1 2 3]';

sigvx=0.002;
sigvy=0.002;
sigvz=0.002;

%% Measurement
% Velocity in the body coord V_ibB
Vx=z(1);
Vy=z(2);
Vz=z(3);

nuVx=0;
nuVy=0;
nuVz=0;

%% Navigation State
La=x(1);
Lo=x(2);
Z=x(3);

% v_bnN
Vn=x(4);
Ve=x(5);
Vd=x(6);

phi=x(7);
theta=x(8);
psi=x(9);

%% Data preprocessing
C_bn= DCM([phi, theta, psi]);
 
%% Constructing the system matrix
%state propogation matrix, size:9*9
C11=C_bn(1,1);
C12=C_bn(1,2);
C13=C_bn(1,3);
C21=C_bn(2,1);
C22=C_bn(2,2);
C23=C_bn(2,3);
C31=C_bn(3,1);
C32=C_bn(3,2);
C33=C_bn(3,3);

% predicted measurement, nonlinear function 
Vxbar=C11*Vn+C21*Ve+C31*Vd+nuVx;
Vybar=C12*Vn+C22*Ve+C32*Vd+nuVy;
Vzbar=C13*Vn+C23*Ve+C33*Vd+nuVz;

% innovation
Vxtilde=Vx-Vxbar;
Vytilde=Vy-Vybar;
Vztilde=Vz-Vzbar;

De_C11_phi=0;
De_C12_phi=sin(phi)*sin(psi)+cos(phi)*sin(theta)*cos(psi);
De_C13_phi=cos(phi)*sin(psi)-sin(phi)*sin(theta)*cos(psi);
De_C21_phi=0;
De_C22_phi=-sin(phi)*cos(psi)+cos(phi)*sin(theta)*sin(psi);
De_C23_phi=-cos(phi)*cos(psi)-sin(phi)*sin(theta)*sin(psi);
De_C31_phi=0;
De_C32_phi=cos(phi)*cos(theta);
De_C33_phi=-sin(phi)*cos(theta);

De_C11_theta=-sin(theta)*cos(psi);
De_C12_theta=sin(phi)*cos(theta)*cos(psi);
De_C13_theta=cos(phi)*cos(theta)*cos(psi);
De_C21_theta=-sin(theta)*sin(psi);
De_C22_theta=sin(phi)*cos(theta)*sin(psi);
De_C23_theta=-cos(phi)*cos(theta)*sin(psi);
De_C31_theta=-cos(theta);
De_C32_theta=-sin(phi)*sin(theta);
De_C33_theta=-cos(phi)*sin(theta);

De_C11_psi=-cos(theta)*sin(psi);
De_C12_psi=-cos(phi)*cos(psi)-sin(phi)*sin(theta)*sin(psi);
De_C13_psi=sin(phi)*cos(psi)-cos(phi)*sin(theta)*sin(psi);
De_C21_psi=cos(theta)*cos(psi);
De_C22_psi=-cos(phi)*sin(psi)+sin(phi)*sin(theta)*cos(psi);
De_C23_psi=sin(phi)*sin(psi)+cos(phi)*sin(theta)*cos(psi);
De_C31_psi=0;
De_C32_psi=0;
De_C33_psi=0;

H17=Vn*De_C11_phi+Ve*De_C21_phi+Vd*De_C31_phi;
H18=Vn*De_C11_theta+Ve*De_C21_theta+Vd*De_C31_theta;
H19=Vn*De_C11_psi+Ve*De_C21_psi+Vd*De_C31_psi;

H27=Vn*De_C12_phi+Ve*De_C22_phi+Vd*De_C32_phi;
H28=Vn*De_C12_theta+Ve*De_C22_theta+Vd*De_C32_theta;
H29=Vn*De_C12_psi+Ve*De_C22_psi+Vd*De_C32_psi;

H37=Vn*De_C13_phi+Ve*De_C23_phi+Vd*De_C33_phi;
H38=Vn*De_C13_theta+Ve*De_C23_theta+Vd*De_C33_theta;
H39=Vn*De_C13_psi+Ve*De_C23_psi+Vd*De_C33_psi;

Hsub=[H17 H18 H19;
    H27 H28 H29;
    H37 H38 H39];

Hdvl=[zeros(3,3) C_bn' Hsub];

Rdvl=diag([sigvx sigvy sigvz]).^2;
 
 
 
 %% Correction
Kdvl=P*Hdvl'/(Hdvl*P*Hdvl'+Rdvl);
x=x+Kdvl*[Vxtilde Vytilde Vztilde]';
P=P-Kdvl*Hdvl*P;

x(7:9)=rem(x(7:9),2*pi);
for i=7:9
    if x(i)>pi
        x(i)=x(i)-2*pi;
    end
end

 end

3 运行结果

4 参考文献

[1]于聪. 基于多元信息融合的AUV惯性导航系统方法研究[D]. 中国海洋大学.

[2]潘学松. 基于SINS/DVL/GPS的AUV组合导航系统关键技术研究[D]. 中国海洋大学.

博主简介：擅长智能优化算法、神经网络预测、信号处理、元胞自动机、图像处理、路径规划、无人机等多种领域的Matlab仿真，相关matlab代码问题可私信交流。

部分理论引用网络文献，若有侵权联系博主删除。