【高度预估】基于matlab卡尔曼滤波和粒子滤波无人机离地高度估计【含Matlab源码 2255期】

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

更多Matlab仿真内容点击👇
Matlab图像处理（进阶版）
路径规划（Matlab）
神经网络预测与分类（Matlab）
优化求解（Matlab）
语音处理（Matlab）
信号处理（Matlab）
车间调度（Matlab）

⛄一、卡尔曼滤波和粒子滤波无人机离地高度估计

1 无人机离地高度估计算法
1.1 离地高度估计基本方案

1. 无人机安装两路距离测量传感器, 传感器能在小型无人机飞行的一般高度正常工作, 且两个传感器的安装位置保证它们能够测量相同的离地高度信息。

2. 两路距离测量传感器以1s的频率向无人机的飞行控制计算机返回离地高度信息。该周期即为本文设计的卡尔曼滤波器的递推周期。

3. 高速数字信号处理器运行卡尔曼滤波器, 对两路传感器返回的信息进行数据融合, 得到离地高度的最优估计。

4. 由于利用卡尔曼滤波对两路传感器进行数据融合得到的估计结果将优于仅用任何一路传感器的结果, 因此, 本文采用基于卡尔曼滤波的两路距离传感器数据融合算法方案, 具有高可靠性、高精度的特点。

1.2 算法步骤及实现特性
1.2.1 系统模型
建立良好的系统状态模型, 是设计优良的卡尔曼滤波器的基础。考虑无人机在测量离地高度时的垂直方向机动因素, 选取垂直方向的距离、速度、加速度作为状态量, 选择两个距离测量传感器的测量值为观测量。

设tk时刻的距离为dk, 速度为vk, 加速度为ak, 加加速度为jk, 两个距离测量传感器的测量值分别为Zk1, Zk2, 测量噪声为Vk1, Vk2, 测量采样周期为T。

其中, jk是系统状态变化的随机量, 用零均值的白噪声表示。即得到式 (1) :


1.2.2 卡尔曼滤波离地高度估计算法步骤

1. 预测过程

由系统状态方程公式 (5) , 得到系统的状态一步预测[6,8], 见式 (9) :

式中q即为公式 (1) 所示的系统状态随机量。预测协方差表征了系统状态预测的误差程度。

2. 状态更新过程

滤波增益计算[8]见式 (11) :

⛄二、部分源代码

% 为 terpcom 应用程序创建 KF 的主文件
function main()
% 一般命令
close all;
clear all;
clc;

DEBUG =0;

figure(1); hold on;

% 初始化
dt = 0.1; % 时间
bet = 1/10; %贝塔
A = [1 0; 0 exp(-betdt)]; % 状态转移矩阵
% state are -> x1=h, x2 = h_err = h_pred - h = h_msl-h_dma - h
% x1dot = w1 ~=N[0, sigma1^2], sigma1 = 20
% x2dot = -betax2+w2 sqrt(2sigma2^2*beta) -> sigma2=45

Xhat = [160 2]';
Phat = [10 0; 0 10];
% 过程噪声
sigma1 = 20dt;
sigma2 = 45dt;

% 测量模型
H = [1 1; 1 0];

% 测量噪声
sigma3 = 10dt;
sigma4 = 20dt;

Q = diag([sigma1 sigma2]);
R = diag([sigma3 sigma4]);

% 无人机初始化
uvel = 10;
xv = uvelcos(0);
yv = uvelsin(0);
zv = 160;

%创建地形
[terrainHeight,terrainTime] = createTerrain;
plot(terrainHeight,‘r’);

%创造真实的东西
hmslTrue = mslDataTrue(zv,terrainTime);
hdmaTrue = dmaDataTrue(terrainHeight);

% 创建 MSL 数据
hmsl = mslData(zv,terrainTime,sigma3/dt);
plot(hmsl,‘b’);
plot(ones(length(hmsl),1)*zv,‘k’);

%创建 DMA 数据
hdma = dmaData(terrainHeight,sigma4/dt);
plot(hdma,‘.b’);

% 粒子过滤器设置
N =100; %粒子数
xp=[];
% 根据初始正态分布制作随机粒子
for ii = 1:N
xp(ii,1)= 60 + sigma1randn;
xp(ii,2)= 2 + sigma2randn;
end

if(DEBUG)
figure();
plot(1,p(:,1),‘*r’,1,p(:,2),‘*b’);
legend(‘initial distribution’);
end

⛄三、运行结果

⛄四、matlab版本及参考文献

1 matlab版本
2014a

2 参考文献
[1]范巧艳.基于卡尔曼滤波的无人机离地高度估计算法[J].电子设计工程. 2018,26(21)

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