使用扩展卡尔曼滤波(EKF)融合激光雷达和雷达数据(Matlab代码实现)

简介: 使用扩展卡尔曼滤波(EKF)融合激光雷达和雷达数据(Matlab代码实现)

💥💥💞💞欢迎来到本博客❤️❤️💥💥


🏆博主优势:🌞🌞🌞博客内容尽量做到思维缜密,逻辑清晰,为了方便读者。


⛳️座右铭:行百里者,半于九十。


📋📋📋本文目录如下:🎁🎁🎁


目录


💥1 概述


📚2 运行结果


🎉3 参考文献


🌈4 Matlab代码实现


💥1 概述

大多数自动驾驶汽车都配备了激光雷达和雷达。然而,这两者的输出是不同的,激光雷达的输出是笛卡尔坐标中物体的位置,而雷达给出物体在极坐标中的位置和速度。使用扩展卡尔曼滤波器是因为它可以融合非线性数据,在这种情况下是来自笛卡尔坐标和极坐标的数据。为了估计非线性测量,引入了雅可比矩阵。


📚2 运行结果


553b30e7f33547fabf6729f2997aceff.png


主函数部分代码:

clear;
clf;
dt = 0.1;
Data = csvread('Radar_Lidar_Data1.csv',1,1);
% Data = csvread('Radar_Lidar_Data2.csv',1,1);
Radar_Measurement = [];
Lidar_Measurement = [];
EKF_Path = [];
F = [[1, 0, dt, 0];
     [0, 1, 0, dt];
     [0, 0, 1, 0];
     [0, 0, 0, 1]];
u = 0;
B = [(dt^2)/2 (dt^2)/2 dt dt]';
P = [[1, 0, 0, 0];
     [0, 1, 0, 0];
     [0, 0, 1000, 0];
     [0, 0, 0, 1000]];
R_l = [[0.0025, 0];
       [0, 0.0025]];
R_r = [[0.09, 0, 0];
      [0, 0.005, 0];
      [0, 0, 0.09]];
Q = [(dt^2)/4 0 (dt^3)/2 0;
     0 (dt^2)/4 0 (dt^3)/2;
     (dt^3/2) 0 (dt^2) 0;
     0 (dt^3)/2 0 (dt^2)];
H = [[1, 0, 0, 0];
     [0, 1, 0, 0]];
I = eye(4);
if (Data(1,1) == 1)
    x = [Data(1,2); Data(1,3); 0; 0];
else
    x = [Data(1,2); Data(1,3); Data(1,4); 0];
end
for n = 1:length(Data)
    if (Data(n,1) == 2)
        %prediction
        x = F * x + B*u;
        P = F * P * transpose(F) + Q;
        %measurement update
        Z = Data(n,2:4);
        X = Z(1)*cos(Z(2));
        Y = Z(1)*sin(Z(2));
        VX = Z(3)*cos(Z(2));
        VY = Z(3)*sin(Z(2));
        c1 = X^2 + Y^2;
        c2 = sqrt(c1);
        c3 = c1 * c2;
        if (c1==0 || c2==0 || c3==0)
            H_Jac = [[0, 0, 0, 0];
                     [0, 0, 0, 0];
                     [0, 0, 0, 0]];
        else
            H_Jac = [[X/c2, Y/c2, 0, 0];
                    [-Y/c1, X/c1, 0, 0];
                    [(Y*(VX*Y-VY*X))/c3, (X*(X*VY-Y*VX))/c3, X/c2, Y/c2]];
        end
        Z_Car = [X; Y; VX; VY];
        y = transpose(Z) - (H_Jac * Z_Car);
        S = H_Jac * P * transpose(H_Jac) + R_r;
        K = P * transpose(H_Jac) * inv(S);
        x = Z_Car + (K * y);
        P = (I - (K * H_Jac)) * P;
        EKF_Path = [EKF_Path;[x(1),x(2)]];
        Radar_Measurement = [Radar_Measurement; Data(n,2:4)];
    else
        %prediction
        x = (F * x) + B*u;
        P = F * P * transpose(F) + Q;
        %measurement update
        Z = Data(n,2:3);
        y = transpose(Z) - (H * x);
        S = H * P * transpose(H) + R_l;
        K = P * transpose(H) * inv(S);
        x = x + (K * y);
        P = (I - (K * H)) * P;
        EKF_Path = [EKF_Path;[x(1),x(2)]];
        Lidar_Measurement = [Lidar_Measurement; Data(n,2:3)];
    end
end


🎉3 参考文献

[1]杨春玲,倪晋麟,刘国岁,郑全战.转换坐标卡尔曼滤波器的雷达目标跟踪[J].电子学报,1999(03):121-123.

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


🌈4 Matlab代码实现

相关文章
|
8月前
|
传感器 算法 Go
基于EKF扩展卡尔曼滤波的传感器网络目标跟踪matlab仿真
基于EKF扩展卡尔曼滤波的传感器网络目标跟踪matlab仿真
|
4月前
|
资源调度 算法
基于迭代扩展卡尔曼滤波算法的倒立摆控制系统matlab仿真
本课题研究基于迭代扩展卡尔曼滤波算法的倒立摆控制系统,并对比UKF、EKF、迭代UKF和迭代EKF的控制效果。倒立摆作为典型的非线性系统,适用于评估不同滤波方法的性能。UKF采用无迹变换逼近非线性函数,避免了EKF中的截断误差;EKF则通过泰勒级数展开近似非线性函数;迭代EKF和迭代UKF通过多次迭代提高状态估计精度。系统使用MATLAB 2022a进行仿真和分析,结果显示UKF和迭代UKF在非线性强的系统中表现更佳,但计算复杂度较高;EKF和迭代EKF则更适合维数较高或计算受限的场景。
|
8月前
|
算法
MATLAB|【免费】融合正余弦和柯西变异的麻雀优化算法SCSSA-CNN-BiLSTM双向长短期记忆网络预测模型
这段内容介绍了一个使用改进的麻雀搜索算法优化CNN-BiLSTM模型进行多输入单输出预测的程序。程序通过融合正余弦和柯西变异提升算法性能,主要优化学习率、正则化参数及BiLSTM的隐层神经元数量。它利用一段简单的风速数据进行演示,对比了改进算法与粒子群、灰狼算法的优化效果。代码包括数据导入、预处理和模型构建部分,并展示了优化前后的效果。建议使用高版本MATLAB运行。
|
8月前
|
传感器 运维 机器人
matlab实现扩展卡尔曼滤波(EKF)进行故障检测
matlab实现扩展卡尔曼滤波(EKF)进行故障检测
|
8月前
|
编解码 算法
基于双树复小波变换和稀疏表示的多光谱和彩色图像融合算法matlab仿真
基于双树复小波变换和稀疏表示的多光谱和彩色图像融合算法matlab仿真
|
8月前
|
机器学习/深度学习 负载均衡 JavaScript
m基于Q-Learning强化学习的异构网络小区范围扩展(CRE)技术matlab仿真
m基于Q-Learning强化学习的异构网络小区范围扩展(CRE)技术matlab仿真
276 1
|
8月前
|
算法
基于稀疏表示的小波变换多光谱图像融合算法matlab仿真
基于稀疏表示的小波变换多光谱图像融合算法matlab仿真
|
传感器 算法 自动驾驶
基于uwb和IMU融合的三维空间定位算法matlab仿真
基于uwb和IMU融合的三维空间定位算法matlab仿真
|
机器学习/深度学习 传感器 算法
【红外图像】利用红外图像处理技术对不同制冷剂充装的制冷系统进行性能评估(Matlab代码实现)
【红外图像】利用红外图像处理技术对不同制冷剂充装的制冷系统进行性能评估(Matlab代码实现)
|
机器学习/深度学习 传感器 算法
【视频去噪】基于全变异正则化最小二乘反卷积是最标准的图像处理、视频去噪研究(Matlab代码实现)
【视频去噪】基于全变异正则化最小二乘反卷积是最标准的图像处理、视频去噪研究(Matlab代码实现)

热门文章

最新文章

AI助理

你好,我是AI助理

可以解答问题、推荐解决方案等