【目标定位】扩展卡尔曼滤波EKF GPS-IMU组合定位【含Matlab源码 3863期】

最新推荐文章于 2024-06-17 12:14:23 发布

Matlab武动乾坤

最新推荐文章于 2024-06-17 12:14:23 发布

阅读量1.4k

点赞数 47

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

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

版权

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

529 篇文章 47 订阅

订阅专栏

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

⛄一、扩展卡尔曼滤波EKF GPS-IMU组合定位简介

扩展卡尔曼滤波（EKF）是一种常用的滤波算法，用于将GPS和IMU数据进行融合，实现GPS-IMU组合定位。在融合过程中，IMU的姿态解算被用作轨迹增量的预测，而EKF滤波器则用于对导航信息中的误差进行滤波。

EKF的基本思想是通过线性化的方式将非线性系统近似为线性系统，然后使用卡尔曼滤波算法进行状态估计和误差校正。具体步骤如下：
（1）初始化状态和协方差矩阵：根据初始的GPS位置和IMU姿态解算结果，初始化系统的状态和协方差矩阵。
（2）预测状态和协方差：使用IMU的测量值和姿态解算结果，通过运动模型预测系统的状态和协方差。
（3）更新状态和协方差：使用GPS的测量值，将预测的状态和协方差更新为更准确的估计值。
（4）重复步骤2和步骤3：根据IMU的测量值和姿态解算结果，不断预测和更新系统的状态和协方差。

⛄二、部分源代码

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%程序初始化操作%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clc;
clear;
close all;

iBeacon_IMU_location = ParticleFilter();
GPS_IMU_location = GPS_IMU_PF();
Adopted_location = zeros(2,361);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%全局变量定义%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
outdoor_sensor_data=361;
indoor_sensor_data=0;
sensor_data=outdoor_sensor_data+indoor_sensor_data;
d=0.1;%标准差
Theta=CreateGauss(0,d,1,sensor_data);%GPS航迹和DR航迹的夹角
ZOUT=zeros(4,outdoor_sensor_data);
ZIN=zeros(4,indoor_sensor_data);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%读取传感器数据%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
fgps=fopen(‘sensor_data_041518.txt’,‘r’);%%%打开文本

for n=1:sensor_data
gpsline=fgetl(fgps);%%%读取文本指针对应的行
if ~ischar(gpsline) break;%%%判断是否结束
end;
%%%%读取室内数据
time=sscanf(gpsline,‘[Info] 2016-04-15%s(ViewController.m:%d)-[ViewController outputAccelertion:]:lat:%f;lon:%f;heading:%f;distance:%f;beacon_lat:%f;beacon_lon:%f’);
data=sscanf(gpsline,‘[Info] 2016-04-15 %*s (ViewController.m:%*d)-[ViewController outputAccelertion:]:lat:%f;lon:%f;heading:%f;distance:%f;beacon_lat:%f;beacon_lon:%f’);
if(isempty(data))
break;
end
result=lonLat2Mercator(data(6,1),data(5,1));
gx(n)=result.X;%GPS经过坐标变换后的东向坐标，换算成米数
gy(n)=result.Y;%GPS经过坐标变换后的北向坐标，换算成米数
Phi(n)=(data(3,1)+90)*pi/180;%航向角
dd(n)=data(4,1);%某一周期的位移
ZIN(:,n)=[gx(n),gy(n),Phi(n),dd(n)];
if ZIN(1,n) == 0
Adopted_location(1,n) = GPS_IMU_location(1,n);
Adopted_location(2,n) = GPS_IMU_location(2,n);
else
Adopted_location(1,n) = iBeacon_IMU_location(1,n);
Adopted_location(2,n) = iBeacon_IMU_location(2,n);
end
end
fclose(fgps);%%%%%关闭文件指针

cordinatex=round(ZIN(1,5));
cordinatey=round(ZIN(2,5));

[groundtruthx,groundtruthy]=Groud_Truth();
groundtruth = [groundtruthx,groundtruthy]';
iBeacon_IMU_location_line=iBeacon_IMU_location(:,2:361)-groundtruth(:,2:361);
iBeacon_IMU_location_error=sqrt(iBeacon_IMU_location_line(1,:).^{2+iBeacon_IMU_location_line(2,:).}2);
GPS_IMU_location_line=GPS_IMU_location(:,2:361)-groundtruth(:,2:361);
GPS_IMU_location_error=sqrt(GPS_IMU_location_line(1,:).^{2+GPS_IMU_location_line(2,:).}2);
Adopted_location_line=Adopted_location(:,2:361)-groundtruth(:,2:361);
Adopted_location_error=sqrt(Adopted_location_line(1,:).^{2+Adopted_location_line(2,:).}2);

x_Adopted_location = zeros(1,11);
c_Adopted_location = zeros(1,11);
[b_Adopted_location, x_Adopted_location(1,2:11)]=hist(Adopted_location_error,10);
num=numel(Adopted_location_error);
%figure;plot(x_Adopted_location(1,2:11),b_Adopted_location/num); %概率密度
c_Adopted_location(1,2:11)=cumsum(b_Adopted_location/num); %累积分布

x_iBeacon_IMU_location = zeros(1,11);
c_iBeacon_IMU_location = zeros(1,11);
[b_iBeacon_IMU_location, x_iBeacon_IMU_location(1,2:11)]=hist(iBeacon_IMU_location_error,10);
num=numel(iBeacon_IMU_location_error);
%figure;plot(x_Adopted_location(1,2:11),b_Adopted_location/num); %概率密度
c_iBeacon_IMU_location(1,2:11)=cumsum(b_iBeacon_IMU_location/num); %累积分布

x_GPS_IMU_location = zeros(1,11);
c_GPS_IMU_location = zeros(1,11);
[b_GPS_IMU_location, x_GPS_IMU_location(1,2:11)]=hist(GPS_IMU_location_error,10);
num=numel(GPS_IMU_location_error);
%figure;plot(x_Adopted_location(1,2:11),b_Adopted_location/num); %概率密度
c_GPS_IMU_location(1,2:11)=cumsum(b_GPS_IMU_location/num); %累积分布

figure(1);
plot(x_Adopted_location,c_Adopted_location,‘r’);hold on;
plot(x_iBeacon_IMU_location,c_iBeacon_IMU_location,‘b’);hold on;
plot(x_GPS_IMU_location,c_GPS_IMU_location,‘g’);hold off;
legend(‘iBeacon/IMU/GPS定位’, ‘iBeacon/IMU定位’,‘GPS/IMU定位’);
xlabel(‘定位误差/m’, ‘FontSize’, 10); ylabel(‘累积概率分布/%’, ‘FontSize’, 10);

figure(2);
set(gca,‘FontSize’,12);
plot(groundtruthx,groundtruthy,‘r’);hold on;
%plot( ZIN(1,:), ZIN(2,:), ‘o’);hold on;
plot(Adopted_location(1,:), Adopted_location(2,:), ‘g’);hold off;
axis([cordinatex-100 cordinatex+200 cordinatey-200 cordinatey+100]),grid on;
legend(‘真实轨迹’, ‘粒子滤波轨迹’);
xlabel(‘x’, ‘FontSize’, 20); ylabel(‘y’, ‘FontSize’, 20);
axis equal;

⛄三、运行结果

在这里插入图片描述

⛄四、matlab版本及参考文献

1 matlab版本
2014a

2 参考文献
[1]李永生,董光焰,陈凯,谢亚峰,唐金力.基于卡尔曼滤波的无源定位精度分析[J].弹箭与制导学报. 2022,42(04)

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