1.算法描述

EKF扩展卡尔曼滤波要解决的是卡尔曼滤波不适用于非线性模型的问题。其和卡尔曼滤波算法结构相同，只是将非线性模型线性化，然后再应用卡尔曼滤波完成状态估计。

扩展卡尔曼滤波的初始化，需要将各个变量进行设置，对于不同的运动模型，状态向量是不一样的。为了保证代码对不同状态向量的兼容性，我们使用Eigen库中非定长的数据结构。初始化扩展卡尔曼滤波器时需要输入一个初始的状态量x_in，用以表示障碍物最初的位置和速度信息，一般直接使用第一次的测量结果。这两个公式求的是卡尔曼滤波器中一个很重要的量——卡尔曼增益K（Kalman Gain），用人话讲就是求差值y的权值。第一个公式中的R是测量噪声矩阵（measurement covariance matrix），这个表示的是测量值与真值之间的差值。一般情况下，传感器的厂家会提供。

扩展卡尔曼（EKF）与经典卡尔曼（KF）的区别在于测量矩阵H的计算。EKF对非线性函数进行泰勒展开后，进行一阶线性化的截断，忽略了其余高阶项，进而完成非线性函数的近似线性化。正是由于忽略了部分高阶项，使得EKF的状态估计会损失一些精度。

卡尔曼滤波作为连续状态空间问题的一种解决方案，已经成功运用在火星登陆和自动导弹制导等领域。本质上卡尔曼滤波(Kalman Filter)是一置信度为正态分布的贝叶斯(Bayes Filter)滤波器，它的置信度可以表示为一个均值向量和协方差矩阵的形式，均值向量表示可能的状态，协方差矩阵表示该状态的不确定度。

前提假设

卡尔曼滤波需要满足以下三个前提假设，才能保证在任意一个时刻t的置信度bel(x(t))都是正态分布：

1) 运动模型必须是一个线性高斯模型。也就是说t+1时刻的状态必须是t时刻的状态的线性函数，附加一个高斯随机噪声。

其中A是一个矩阵，Δ是运动向量，ξ是高斯随机噪声

2) 传感器模型也是一个线性高斯模型。

其中是一个矩阵，ζ表示传感器测量的高斯噪声。

3) 初始置信度bel(x0)是一个正态分布。

一维状态空间卡尔曼滤波

先从最简单的一维卡尔曼滤波说起，假设一辆小车以δ的速度和一定的随机噪声沿着一条直线前进，则它的运动方程：

针对于线性高斯系统，卡尔曼滤波高效、简单、易行，然而遗憾的是在实际问题中，这样的理想的场景非常少。于是又有了扩展卡尔曼滤波，扩展卡尔曼滤波的思想是非线性系统在均值的一个小的邻域内可以近似认为是线性的，从而可以得到一个近似的高斯置信度。

2.仿真效果预览

matlab2022a仿真结果如下：

3.MATLAB核心程序

%测试传感器融合跟踪
%在自我载具左侧经过的载具。场景
%模拟公路设置，其他车辆位于和前面
%在自我汽车后面。
scenario = drivingScenario;
scenario.SampleTime = 0.01;
% Rear-left-facing short-range radar sensor at the left rear wheel well of the car.
sensors{3} = radarDetectionGenerator('SensorIndex', 3, 'Height', 0.2, 'Yaw', 120, ...'SensorLocation', [0, egoCar.Width/2], 'MaxRange', 30, 'ReferenceRange', 50, ...'FieldOfView', [90, 5], 'AzimuthResolution', 10, 'RangeResolution', 1.25);% Rear-right-facing short-range radar sensor at the right rear wheel well of the car.
sensors{4} = radarDetectionGenerator('SensorIndex', 4, 'Height', 0.2, 'Yaw', -120, ...'SensorLocation', [0, -egoCar.Width/2], 'MaxRange', 30, 'ReferenceRange', 50, ...'FieldOfView', [90, 5], 'AzimuthResolution', 10, 'RangeResolution', 1.25);% Front-left-facing short-range radar sensor at the left front wheel well of the car.
sensors{5} = radarDetectionGenerator('SensorIndex', 5, 'Height', 0.2, 'Yaw', 60, ...'SensorLocation', [egoCar.Wheelbase, egoCar.Width/2], 'MaxRange', 30, ...'ReferenceRange', 50, 'FieldOfView', [90, 5], 'AzimuthResolution', 10, ...'RangeResolution', 1.25);% Front-right-facing short-range radar sensor at the right front wheel well of the car.
sensors{6} = radarDetectionGenerator('SensorIndex', 6, 'Height', 0.2, 'Yaw', -60, ...'SensorLocation', [egoCar.Wheelbase, -egoCar.Width/2], 'MaxRange', 30, ...'ReferenceRange', 50, 'FieldOfView', [90, 5], 'AzimuthResolution', 10, ...'RangeResolution', 1.25);
%%创建跟踪程序%创建一个|<matlab:doc（'multiObjectTracker'）multiObjectTracker>|来跟踪%靠近小我车的车辆。跟踪器使用%|initSimDemoFilter|支持函数初始化恒速%处理位置和速度的线性卡尔曼滤波器。%%跟踪以二维方式进行。虽然传感器返回三维测量值，%运动本身仅限于水平面，因此没有%需要跟踪高度。tracker = multiObjectTracker('FilterInitializationFcn', @initSimDemoFilter, ...'AssignmentThreshold', 30, 'ConfirmationParameters', [4 5], 'NumCoastingUpdates', 5);
positionSelector = [1 0 0 0; 0 0 1 0]; % Position selector
velocitySelector = [0 1 0 0; 0 0 0 1]; % Velocity selectorBEP = createDemoDisplay(egoCar, sensors);toSnap = true;
while advance(scenario) && ishghandle(BEP.Parent)    % Get the scenario timetime = scenario.SimulationTime;% Get the position of the other vehicle in ego vehicle coordinatesta = targetPoses(egoCar);% Simulate the sensorsdetections = {};isValidTime = false(1,6);for i = 1:6[sensorDets,numValidDets,isValidTime(i)] = sensors{i}(ta, time);if numValidDetsdetections = [detections; sensorDets]; %#ok<AGROW>endend% Update the tracker if there are new detectionsif any(isValidTime)vehicleLength = sensors{1}.ActorProfiles.Length;detectionClusters = clusterDetections(detections, vehicleLength);confirmedTracks = updateTracks(tracker, detectionClusters, time);% Update bird's-eye plotupdateBEP(BEP, egoCar, detections, confirmedTracks, positionSelector, velocitySelector);end% Snap a figure for the document when the car passes the ego vehicleif ta(1).Position(1) > 0 && toSnaptoSnap = false;snapnowend
end
A111

4.完整MATLAB

基于扩展卡尔曼滤波的自适应智能无人车辆行驶matlab仿真,输出三维动态行驶动画相关推荐

matlab lms自适应滤波,基于LMS算法的自适应滤波收敛性的Matlab仿真
Author :Jeffrey 白噪声经过AR模型的输出作为LMS滤波器的输入,已知:a1=1.558:a2=-0.81:白噪声方差为1.0,均值为0:u＝0.002:利用Matlab实现: (1)给 ...
基于容积卡尔曼滤波(CubatureKalmam Filter, CKF)的车辆状态观测器 Carsim与Simulink联合可生成C代码
基于容积卡尔曼滤波(CubatureKalmam Filter, CKF)的车辆状态观测器 Carsim与Simulink联合可生成C代码 ?CKF算法使用子函数形式编程,在定义好状态方程和观测方程 ...
python卡尔曼滤波室内定位_基于扩展卡尔曼滤波算法的室内定位跟踪系统
基于扩展卡尔曼滤波算法的室内定位跟踪系统凌海波,周先存 [摘要] 摘要:为了解决无线室内定位系统实时跟踪位置坐标误差较大问题, 提出一种基于扩展卡尔曼滤波 (EKF) 算法的室内定位方法.系统采用 ...
基于扩展卡尔曼滤波EKF和模型预测控制MPC，自动泊车场景建模开发
基于扩展卡尔曼滤波EKF和模型预测控制MPC,自动泊车场景建模开发,文复现. MATLAB 基于扩展卡尔曼滤波EKF和模型预测控制MPC,自动泊车场景建模开发,文复现. MATLAB(工程项目线上支持 ...
扩展卡尔曼滤波（EKF）算法详细推导及仿真（Matlab）
前言扩展卡尔曼滤波算法是解决非线性状态估计问题最为直接的一种处理方法,尽管EKF不是最精确的"最优"滤波器,但在过去的几十年成功地应用到许多非线性系统中.所以在学习非线性滤波问题 ...
基于matlab的ldpc编码的构造,基于LDPC编码的GMSK调制与解调及matlab仿真实现(含录像)...
基于LDPC编码的GMSK调制与解调及matlab仿真实现(含录像)(开题报告,论文10700字,程序代码,录像) 摘要随着无线通信技术的不断发展与进步,数字电视广播.移动视频点播等对数据吞吐量要 ...
Matlab算法DSP移植验证,DSP计算机作业自适应噪声抵消LMS算法Matlab仿真
[实例简介] 自适应噪声抵消LMS算法Matlab仿真,DSP计算机作业数字信号处理自适应 1) 借助MATLAB画出误差性能曲面和误差性能曲面的等值曲线: 2) 写出最陡下降法, LMS算法的计 ...
matlab中对伺服电机,基于MATLAB仿真的伺服电机动态特性分析
基于MATLAB仿真的伺服电机动态特性分析摘要:伺服电机(servo motor )是指在伺服系统中控制机械元件运转的发动机,是一种补助马达间接变速装置.伺服电机可使控制速度,位置精度非常准确,可以 ...
基于扩展卡尔曼滤波(EKF)的机器人状态估计
点击上方"小白学视觉",选择加"星标"或"置顶" 重磅干货,第一时间送达 EKF的目的是使卡尔曼滤波器能够应用于机器人等非线性运动系统,EK ...

基于扩展卡尔曼滤波的自适应智能无人车辆行驶matlab仿真,输出三维动态行驶动画

1.算法描述

2.仿真效果预览

3.MATLAB核心程序

4.完整MATLAB

基于扩展卡尔曼滤波的自适应智能无人车辆行驶matlab仿真,输出三维动态行驶动画相关推荐

最新文章

热门文章