【信息融合】扩展卡尔曼滤波雷达数据和红外数据融合【含Matlab源码 3596期】

最新推荐文章于 2024-05-05 21:04:07 发布

Matlab武动乾坤

最新推荐文章于 2024-05-05 21:04:07 发布

阅读量36

点赞数

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

本文链接：https://blog.csdn.net/KeepingMatlab/article/details/135123542

版权

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

528 篇文章 46 订阅

订阅专栏

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

⛄一、卡尔曼滤波kalman信息融合

1 kalman原理
卡尔曼滤波是一种递推式滤波方法，不须保存过去的历史信息，新数据结合前一刻已求得的估计值及系统本身的状态方程按一定方式求得新的估计值。

1.2 扩展卡尔曼
实际系统总是存在不同程度的非线性，对于非线性系统滤波问题，常用的处理方法是利用线性化技巧将其转化为一个近似的线性滤波问题，这就是扩展Kalman 滤波方法(Extended Kalman Filter,EKF )思路。扩展Kalman 滤波建立在线性Kalman 滤波的基础上，其核心是对一般的非线性系统将滤波值非线性函数f()和h()展开成Taylor级数并略去二阶及以上项，得到一个近似的线性化模型，然后应用Kalman 滤波完成对目标的滤波估计处理。

1.3、无迹卡尔曼
扩展Kalman滤波是对非线性的系统方程或者观测方程进行泰勒展开并保留其一阶近似项，不可避免地引入了线性化误差。如果线性化假设不成立，采用这算法则会导致滤波器性能下降以至于造成发散。无迹Kalman 滤波(Unscented Kalman Filter,UKF )摒弃了对非线性函数进行线性化的传统做法，采用Kalman 线性滤波框架，对于预测方程，使用无迹变换(Unscented Transform, UT) 来处理均值和协方差的非线性传递问题。

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

⛄二、部分源代码

% % 融合
% % 同步后数据的融合
% %
% % %-----------------------------------------------------------------------------------------------
function y=fusion_asyn;
clear;
clc;
[ZTrue]=source;

Tf=0.020;%融合周期
TimesOut=fix(10/Tf);
%-------------误差计算-----------------------------------
ErrSquareSita=zeros(TimesOut,1) %方位角误差平方和变量
ErrSquareSita_w=zeros(TimesOut,1); %方位角速度误差平方和变量
ErrSquareBeta=zeros(TimesOut,1) %俯仰角误差平方和变量
ErrSquareBeta_w=zeros(TimesOut,1); %俯仰角速度误差平方和变量

ErrSquareSita1=zeros(TimesOut,1) %方位角误差平方和变量
ErrSquareSita_w1=zeros(TimesOut,1); %方位角速度误差平方和变量
ErrSquareBeta1=zeros(TimesOut,1) %俯仰角误差平方和变量
ErrSquareBeta_w1=zeros(TimesOut,1); %俯仰角速度误差平方和变量

ErrSquareSita2=zeros(TimesOut,1) %方位角误差平方和变量
ErrSquareSita_w2=zeros(TimesOut,1); %方位角速度误差平方和变量
ErrSquareBeta2=zeros(TimesOut,1) %俯仰角误差平方和变量
ErrSquareBeta_w2=zeros(TimesOut,1); %俯仰角速度误差平方和变量

mont=10;
for m=1:mont
[Xasyn_Radar,Zasyn_Radar,Pasyn_Radar]=Asynchronism_Radar;
[Xasyn_IR,Zasyn_IR,Pasyn_IR]=Asynchronism_IR;
Xfusion=zeros(6,1,TimesOut);
for i=1:TimesOut
for j=1:6
Xfusion(j,1,i)=(Pasyn_IR(j,j,i)*Xasyn_Radar(j,1,i)+Pasyn_Radar(j,j,i)*Xasyn_IR(j,1,i))/(Pasyn_Radar(j,j,i)+Pasyn_IR(j,j,i));
end
end

Zfusion=zeros(4,1,TimesOut);

%-------------转换为极坐标下值---------------------------------------------------------------
for i=1:TimesOut
%模型观测距离

% range=(X2(1,1,i)^2+X2(3,1,i)2+X2(5,1,i)²⁾0.5;

%模拟观测方位角度 
azimuth=atan(Xfusion(1,1,i)/Xfusion(3,1,i));      %%%%%   这里采用的是孙忠康的雷达数据处理中323页的公式  注意其方位角的公式与一般不同!!!!!!!

%
if azimuth>2pi
azimuth=azimuth-2pi;
else if azimuth < 0
azimuth=azimuth+2*pi;
end;
end;
%观测俯仰角度
pitching=atan(Xfusion(5,1,i)/(Xfusion(1,1,i)^{2+Xfusion(3,1,i)}2)^0.5);
if pitching>pi/2
pitching=pitching-pi;
else if pitching<-pi/2
pitching=pitching+pi;
end;
end;
%观测方位角速度
azimuth_w=(Xfusion(3,1,i)*Xfusion(2,1,i)-Xfusion(1,1,i)*Xfusion(4,1,i))/(Xfusion(1,1,i)^{2+Xfusion(3,1,i)}2) ;

%观测俯仰角角速度
R_Rs=(Xfusion(1,1,i)^2+Xfusion(3,1,i)^2)^0.5;
R_R=(Xfusion(1,1,i)^2+Xfusion(3,1,i)^2+Xfusion(5,1,i)^2)^0.5;
R_V=(Xfusion(1,1,i)*Xfusion(2,1,i)+Xfusion(3,1,i)*Xfusion(4,1,i)+Xfusion(5,1,i)*Xfusion(6,1,i) )/R_R;

pitching_w=( Xfusion(6,1,i)*R_R-Xfusion(5,1,i)R_V)/(R_RR_Rs) ;

Zfusion(:,:,i)=[%range;
          azimuth;
          pitching;
          azimuth_w;
          pitching_w];

end;
%-------------转换为极坐标系下值结束-------------

for i=1:TimesOut
ErrSquareSita(i)=(ZTrue(1,1,i)-Zfusion(1,1,i)).^2+ErrSquareSita(i);
ErrSquareBeta(i)=(ZTrue(2,1,i)-Zfusion(2,1,i)).^2+ErrSquareBeta(i);
ErrSquareSita_w(i)=(ZTrue(3,1,i)-Zfusion(3,1,i)).^2+ErrSquareSita_w(i);
ErrSquareBeta_w(i)=(ZTrue(4,1,i)-Zfusion(4,1,i)).^2+ErrSquareBeta_w(i);

ErrSquareSita1(i)=(ZTrue(1,1,i)-Zasyn_Radar(1,1,i)).^2+ErrSquareSita1(i);
ErrSquareBeta1(i)=(ZTrue(2,1,i)-Zasyn_Radar(2,1,i)).^2+ErrSquareBeta1(i);
ErrSquareSita_w1(i)=(ZTrue(3,1,i)-Zasyn_Radar(3,1,i)).^2+ErrSquareSita_w1(i);
ErrSquareBeta_w1(i)=(ZTrue(4,1,i)-Zasyn_Radar(4,1,i)).^2+ErrSquareBeta_w1(i);

ErrSquareSita2(i)=(ZTrue(1,1,i)-Zasyn_IR(1,1,i)).^2+ErrSquareSita2(i);
ErrSquareBeta2(i)=(ZTrue(2,1,i)-Zasyn_IR(2,1,i)).^2+ErrSquareBeta2(i);
ErrSquareSita_w2(i)=(ZTrue(3,1,i)-Zasyn_IR(3,1,i)).^2+ErrSquareSita_w2(i);
ErrSquareBeta_w2(i)=(ZTrue(4,1,i)-Zasyn_IR(4,1,i)).^2+ErrSquareBeta_w2(i);

end
end
% for i=2:TimesOut
% A(i)=sqrt(ErrSquareSita(i)/mont);
% B(i)=sqrt(ErrSquareBeta(i)/mont);
% C(i)=sqrt(ErrSquareSita_w(i)/mont);
% D(i)=sqrt(ErrSquareBeta_w(i)/mont);
%
% A1(i)=sqrt(ErrSquareSita1(i)/mont);
% B1(i)=sqrt(ErrSquareBeta1(i)/mont);
% C1(i)=sqrt(ErrSquareSita_w1(i)/mont);
% D1(i)=sqrt(ErrSquareBeta_w1(i)/mont);
%
% A2(i)=sqrt(ErrSquareSita2(i)/mont);
% B2(i)=sqrt(ErrSquareBeta2(i)/mont);
% C2(i)=sqrt(ErrSquareSita_w2(i)/mont);
% D2(i)=sqrt(ErrSquareBeta_w2(i)/mont);
%
% end
for i=2:TimesOut
A(i-1)=sqrt(ErrSquareSita(i)/mont);
B(i-1)=sqrt(ErrSquareBeta(i)/mont);
C(i-1)=sqrt(ErrSquareSita_w(i)/mont);
D(i-1)=sqrt(ErrSquareBeta_w(i)/mont);

A1(i-1)=sqrt(ErrSquareSita1(i)/mont);
B1(i-1)=sqrt(ErrSquareBeta1(i)/mont);
C1(i-1)=sqrt(ErrSquareSita_w1(i)/mont);
D1(i-1)=sqrt(ErrSquareBeta_w1(i)/mont);

A2(i-1)=sqrt(ErrSquareSita2(i)/mont);
B2(i-1)=sqrt(ErrSquareBeta2(i)/mont);
C2(i-1)=sqrt(ErrSquareSita_w2(i)/mont);
D2(i-1)=sqrt(ErrSquareBeta_w2(i)/mont);

end
%-------------误差计算结束------------------------------

p=2:TimesOut;

plot(p,A,‘r’,p,A1,p,A2);
title(‘方位角误差均方根’);
xlabel(‘t’);
ylabel(‘error /rad’);
legend(‘融合结果’,‘雷达滤波值’,‘红外滤波值’);
figure;
plot(p,B,‘r’,p,B1,p,B2);
title(‘俯仰角误差均方根’);
xlabel(‘t’);
ylabel(‘error /rad’);
legend(‘融合结果’,‘雷达滤波值’,‘红外滤波值’);
figure;
plot(p,C,‘r’,p,C1,p,C2);
title(‘方位角角速率误差均方根’);
xlabel(‘t’);
ylabel(‘error /rad’);
legend(‘融合结果’,‘雷达滤波值’,‘红外滤波值’);
figure;
plot(p,D,‘r’,p,D1,p,D2);
title(‘俯仰角角速率误差均方根’);
xlabel(‘t’);
ylabel(‘error /rad’);
legend(‘融合结果’,‘雷达滤波值’,‘红外滤波值’);

% for i=1:TimesOut
% A(i)=Zasyn_Radar(1,1,i);
% B(i)=ZTrue(1,1,i);
% C(i)=Zasyn_IR(1,1,i);
% end
% plot(p,A,p,B,‘r’,p,C);

⛄三、运行结果

在这里插入图片描述

⛄四、matlab版本及参考文献

1 matlab版本
2014a

2 参考文献
[1]陈寅,林良明,颜国正.D-S证据推理在信息融合应用中的存在问题及改进[J].系统工程与电子技术. 2000,(11)

[2]邓肯·麦克尼尔.多传感器数据融合：基于卡尔曼滤波和神经网络的方法[M].机械工业出版社, 2005

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 雷达方面
卡尔曼滤波跟踪、航迹关联、航迹融合