【信息融合】MDPDA算法水下潜艇与航行器信息融合【含Matlab源码 3599期】

最新推荐文章于 2024-09-14 17:25:17 发布

Matlab领域

最新推荐文章于 2024-09-14 17:25:17 发布

阅读量818

点赞数 22

分类专栏： Matlab信号处理（高阶版）文章标签： matlab

本文链接：https://blog.csdn.net/weixin_63266434/article/details/134905637

版权

Matlab信号处理（高阶版）专栏收录该内容

432 篇文章 14 订阅

订阅专栏

本文介绍了MDPDA算法在水下潜艇与航行器信息融合中的应用，重点讲解了算法原理、多传感器数据融合方法以及部分Matlab源代码示例。通过概率推断提高跟踪精度，同时探讨了惯性导航、声学导航和视觉导航在水下机器人导航中的角色。

摘要由CSDN通过智能技术生成

在这里插入图片描述

⛄一、 MDPDA算法水下潜艇与航行器信息融合

1 数据融合（信息融合）
将经过集成处理的多传感器信息进行合成，形成一种对外部环境或被测对象某一特征的表达方式称为信息融合。常见由以下几种信息融合方法。
（1）综合平均法，即把各传感器数据求平均，乘上权重系数；
（2）贝叶斯估计法，通过先验信息和样本信息得到后验分布，再对目标进行预测；
（3）D-S法，采用概率区间和不确定区间判定多证据体下假设的似然函数进行推理；
（4）神经网络法，以测量目标的各种参数集为神经网络的输入，输出推荐的目标特征或权重；
（5）kalman法、专家系统法等。。。

2 MDPDA算法水下潜艇与航行器信息融合
MDPDA算法是一种基于概率的多假设跟踪算法，主要用于目标跟踪领域。该算法通过对目标的多个假设进行跟踪，从而提高了跟踪的准确性和鲁棒性。该算法的核心思想是将目标跟踪问题转化为一个概率推断问题，通过对目标状态的多个假设进行跟踪，从而得到目标的最优估计。该算法在水下机器人的目标跟踪和探测等领域有着广泛的应用。

关于水下机器人导航与控制：
水下机器人的导航和控制是水下机器人技术中的重要问题。由于水下环境的复杂性和不确定性，水下机器人的导航和控制面临着很多挑战。目前，常用的水下机器人导航和控制方法包括惯性导航、声学导航、视觉导航等。其中，惯性导航是一种基于陀螺仪和加速度计等惯性传感器的导航方法，具有较高的精度和稳定性；声学导航是一种基于声纳和水声信号的导航方法，适用于水下环境中的长距离导航；视觉导航是一种基于摄像头和图像处理技术的导航方法，适用于水下环境中的近距离导航和目标识别等。

⛄二、部分源代码

%%% data association
%Z(k) = [Z1(k);Z2(k)]’ Z1(k) = [Z11(k);Z13(k);Z12(k)] Z2(k) = [Z21(k);Z22(k);Z23(k)]
function [measure_T1,measure_T2,real_n,measure,r_filter,q_filter,v_filter,c_filter,a]=GPDA()
clear all
% close all
clc;
tic
T_submar=1;
T_navigate=2;
T0=T_submar;
n=T_navigate/T_submar;
MC_number=2; %monte carlos run times
%%%%%%%%%%%%%%%%%%%%%%%制导站与航行器的数据进行关联，采用概率数据关联法%%%%%
PD=1; %检测概率
PG=0.99; %正确量测落入跟踪门内的概率
g_sigma=9.21; %门限
lamada =0.05; %错误量测密度
T=T_navigate;
b=0.97; %自动加权卡尔曼滤波中的遗忘因子
%舰船目标
A_target=[1 T 0 0;0 1 0 0;0 0 1 T;0 0 0 1]; %状态转移矩阵
B_target=[T.^2/2 0;T 0;0 T.^2/2;0 T]; %过程噪声矩阵
target_state=[1520 20 1510 15]';
Q_target=[0.004 0;0 0.004]; %过程噪声矩阵

N=100/T;
[real,r_real,Q_real,v_real,C_real,mix_real,target_real]=real_data(T_submar);
for k = 1:N
real_n(1:12,k) = real(:,2k-1);r_real_n(1:3,k) = r_real(:,2k-1);
Q_real_n(1:3,k) = Q_real(:,2k-1);v_real_n(1:3,k) = v_real(:,2k-1);
C_real_n(1:3,k) = C_real(:,2k-1);mix_real_n(:,k) = mix_real(:,2k-1);
d_k(k)=(1-b)/(1-b^(k+1)); %指数权系数
end

[measure_submar,R_submar,C_sub,U_sub,measure_T1]=measure_data(r_real,Q_real,v_real,C_real,mix_real,T_submar,T0,n); %函数调用
[measure_navigate,R_navigate,C_nav,U_nav,measure_T2]=measure_data(r_real_n,Q_real_n,v_real_n,C_real_n,mix_real_n,T,T0,n);

C=zeros(4,4,N,2);
U=zeros(4,1,N,2);
R=zeros(4,4,N,2);

measure(:,:,:,1)=measure_submar;
measure(:,:,:,2)=measure_navigate;
for i = 1:N
C(:,:,i,1) = C_sub(:,:,1,2i-1);
C(:,:,i,2) = C_nav(:,:,1,i); %观测矩阵
U(:,:,i,1) = U_sub(:,:,1,2i-1);
U(:,:,i,2) = U_nav(:,:,1,i); %观测系统的系统误差，非随机系列
R(:,:,i,1) = R_submar(:,:,1,i);
R(:,:,i,2) =R_navigate(:,:,1,i); %观测协方差矩阵
end
Z_pre=zeros(4); %存储两个传感器三个目标的观测预测值
S=zeros(4,4);
A =A_target;B=B_target;
Qp=Q_target;

x_filter=zeros(4,N); %存储3个目标所有时刻的滤波值
x_filter1=zeros(4,N,MC_number);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
P1=zeros(4,4);
P1=[0.001 0.001 0 0;0.001 0.002 0 0;0 0 0.001 0.001;0 0 0.001 0.002]; %初始协方差

x(:,1)=target_state;
for M=1:MC_number
for k = 1 : N
if k==1
x(:,k)=Ax(:,1);
else
x(:,k) = Ax_filter(:,k-1); %预估计k时刻状态变量的值
end
P1_pre = AP1A’ +BQpB’;%对应于预估值的协方差
z1 = C(:,:,k,1)*x(:,k)+U(:,:,k,1);
%求量测噪声协方差矩阵
R(1,2,k,1)=(x(1,k)x(3,k)(P1_pre(1,1)-P1_pre(3,3))+(x(3,k)^2-x(1,k)2)*P1_pre(1,3))/(x(1,k)^2+x(3,k)2)^(3/2);
R(1,3,k,1)=(x(1,k)*x(2,k)*P1_pre(1,2)+x(1,k)*x(4,k)*P1_pre(1,4)+x(2,k)*x(3,k)*P1_pre(2,3)+x(3,k)*x(4,k)P1_pre(3,4))/sqrt((x(1,k)^2+x(3,k)2)(x(2,k)^2+x(4,k)2));
R(1,4,k,1)=(x(1,k)*x(2,k)*P1_pre(1,4)-x(1,k)*x(4,k)*P1_pre(1,2)+x(2,k)*x(3,k)*P1_pre(4,3)-x(3,k)*x(4,k)*P1_pre(2,4))/sqrt(x(1,k)^2+x(3,k)2)/(x(2,k)^2+x(4,k)2);
R(2,3,k,1)=(x(2,k)*x(3,k)*P1_pre(1,2)+x(3,k)*x(4,k)*P1_pre(1,4)-x(2,k)*x(1,k)*P1_pre(2,3)-x(1,k)*x(4,k)*P1_pre(3,4))/sqrt(x(2,k)^2+x(4,k)2)/(x(1,k)^2+x(3,k)2);
R(2,4,k,1)=(x(2,k)*x(3,k)*P1_pre(1,4)-x(3,k)*x(4,k)*P1_pre(1,2)-x(2,k)*x(1,k)*P1_pre(4,3)+x(1,k)*x(4,k)*P1_pre(3,2))/(x(2,k)^2+x(4,k)2)/(x(1,k)^2+x(3,k)2);
R(3,4,k,1)=(x(2,k)x(4,k)(P1_pre(4,4)-P1_pre(2,2))+(x(2,k)^2-x(4,k)2)P1_pre(2,4))/(x(2,k)^2+x(4,k)2)^(3/2);
R(2,1,k,1)=R(1,2,k,1);R(3,1,k,1)=R(1,3,k,1);R(4,1,k,1)=R(1,4,k,1);R(3,2,k,1)=R(2,3,k,1);R(4,2,k,1)=R(2,4,k,1);R(4,3,k,1)=R(3,4,k,1);
s1 = C(:,:,k,1)P1_preC(:,:,k,1)’ + R(:,:,k,1);
K =P1_preC(:,:,k,1)'*inv(s1);

%制导站确认量测
z_k1 = zeros(4,3);
z_1 =[];
m=0;
for i = 1:3
z_k1(:,i)=measure(:,k,i,1);
D=(z_k1(:,i)-z1)'inv(s1)(z_k1(:,i)-z1); %计算残差范数
if D<=g_sigma %把未落入跟踪门范围内的量测清零
z_1=[z_1 z_k1(:,i)];
m=m+1; %记录观测个数

end

end

Bk= lamada*(2*pi)*sqrt(abs(det(s1)))*(1-PD*PG)/PD;
if m==0
   x_filter(:,k)= x(:,k);
   P1=P1;                                                      %无回波的情况
else 
 
E=zeros(1,m);
belta=zeros(1,m);

for i=1:m
    a=(z_1(:,i)-z1)'*inv(s1)*(z_1(:,i)-z1);
    E(i)=E(i)+exp(-a/2);
end 
belta0=Bk/(Bk+sum(E));

%无回波时的关联概率
v=zeros(4,1);
v1=zeros(4,4);
for i=1:m
belta(i)=E(i)/(Bk+sum(E));
%算关联概率

    v=v+belta(i)*(z_1(:,i)-z1);                                  %计算等效新息向量
    v1=v1+belta(i)*(z_1(:,i)-z1)*(z_1(:,i)-z1)';
end
x_filter(:,k)=x(:,k) + K*v;                                       %卡尔曼滤波值
%算协方差
Pc=(eye(4)-K*C(:,:,k,1))*P1_pre;
PP=K*(v1-v*v')*K';
P1=belta0*P1_pre+(1-belta0)*Pc+PP;
end