【状态估计】基于线性卡尔曼滤波器和粒子滤波器无人机估计地形高度（Matlab代码实现）

欢迎来到本博客❤️❤️

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

⛳️座右铭：行百里者，半于九十。

本文目录如下：

目录

1 概述

2 运行结果

3 参考文献

4 Matlab代码实现

---

1 概述

本文模拟了安装在无人机中的距离传感器从地形获得的观测结果，并试图通过嘈杂的观测来估计地面的当前高度。线性卡尔曼滤波和粒子滤波技术都得到了实施和比较。

2 运行结果

 

 

 

部分代码：

% process noise
sigma1 = 20*dt;
sigma2 = 45*dt;

% measurement model 
H = [1 1; 1 0];

% measurement noise
sigma3 = 10*dt;
sigma4 = 20*dt;

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

% UAV initialization
uvel = 10;
xv = uvel*cos(0);
yv = uvel*sin(0);
zv = 160;

% create terrain
[terrainHeight,terrainTime] = createTerrain; 
plot(terrainHeight,'r');

%CREATE TRUE STUFF
hmslTrue = mslDataTrue(zv,terrainTime);
hdmaTrue = dmaDataTrue(terrainHeight);

% create MSL data
hmsl = mslData(zv,terrainTime,sigma3/dt);
plot(hmsl,'b');
plot(ones(length(hmsl),1)*zv,'k');

% create DMA data
hdma = dmaData(terrainHeight,sigma4/dt);
plot(hdma,'.b');

% particle filter settings
N =100; %number os particles
xp=[];
% make the random particles based on initial gaussian distribution
for ii = 1:N
    xp(ii,1)= 60 + sigma1*randn;
    xp(ii,2)= 2 + sigma2*randn;
end

if(DEBUG)
    figure();
    plot(1,p(:,1),'*r',1,p(:,2),'*b')；

3 参考文献

部分理论来源于网络，如有侵权请联系删除。

[1]杜云峰.卡尔曼滤波器在过程估计中的应用[J].机电产品开发与创新,2007(04):142-143.

[2]王涛,谢婧怡,孟丽岩,李勐.基于平方根容积卡尔曼滤波器的非线性模型参数识别[J].黑龙江科技大学学报,2023,33(01):109-115.

[3]HSO (2023). Estimate Terrain Height using Linear Kalman Filter and Particle Filter

4 Matlab代码实现