【直方图滤波】基于直方图滤波用于概率机器人定位研究（Matlab代码实现）

 👨‍🎓个人主页：研学社的博客   

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

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

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

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

目录

💥1 概述

📚2 运行结果

🎉3 参考文献

🌈4 Matlab代码实现

---

💥1 概述

直方图滤波 是 Bayes 滤波 的一种基于网格的实现， 将状态空间分解成多个有限区域 ， 划分后利用直方图滤波算法计算状态向量， 它给出状态向量在每一个区域的概率， 区域划分越精细计算结果就越精确， 本文将水平距离 、 深度划分为 20 m， 4 m 的区间， 结合目标的先验知识和状态更新方程 ， 利用直方图进行逼近， 实现对声源的测距 。

与大多数滤波器一样 ， 直方图滤波也包含预测和更新两步， 下面将进行分析说明 :当声源信号在水中传播时， 产生的压力波动由一个水听器接收，水听器所接收的信号包含关于声源位置和幅度的信息 。

定位是移动机器人导航和控制的基本问题，即智能移动机器人要想在环境中自主运动，最基本的问题就是应用传感器感知的信息确定自己在环境中的位置。无论是在结构化或者非结构化的环境中，机器人都必须了解自身与周围环境的位置关系才能选择和做出决策。可见定位技术对智能移动机器人的实际应用是必不可少的移动机器人定位是指根据先验环境地图信息、机器人位姿（位置和姿态）的当前估计以及传感器观测值等输入信息，经过一定的处理和变换，获得准确的机器人状态。机器人的定位方式取决于所采用的传感器。移动机器人常用的定位传感器有里程计、摄像机、激光测距仪、超声传感器、红外线、微波雷达、陀螺仪、指南针、触觉或接近觉传感器等.

📚2 运行结果

主函数代码：

%-----------------------------
% Establish the time vector
T = 0.01;
N = 1000000;
t = (0:N-1)*T;

%-----
%Set up histogram parameters:
samples = 3;
bins = -samples:.5:samples;

%-----------------
%Initialize starting points
x0=0; 
v0=0;

%-----------------------------
% Generate the accelerations
Avar = 1;  Amean = 0;

Astdv = sqrt(Avar);
At = Astdv*randn(1,N) + Amean;
At = At - mean(At) + Amean;

Ahist0 = hist(At,bins);
Ahist0 = Ahist0/sum(Ahist0);
figure(2); plot(bins,Ahist0,'.-');

%-----------------
%MEASUREMENT PDF:
zbins= -10:2.5:20;
Zvar = 10;  Zmean = 5;

Zstdv = sqrt(Zvar);
Zt = Zstdv*randn(1,N) + Zmean;
Zt = Zt - mean(Zt) + Zmean;

Zhist0 = hist(Zt,zbins);
Zhist0 = Zhist0/sum(Zhist0);
figure(5); plot(zbins,Zhist0,'.-');

%---------------------------
%Timestep 1
%start, x0, v0, Abins, Aprobability, inPoints, inProbability
[movement1, probability1] = HistogramFunction(x0,v0, bins, Ahist0, 0, 0);
figure(3);
subplot(5,1,1);
plot(movement1(1,:), probability1);
xlabel('Location (timestep 1)'); ylabel('Probability');

%Timestep2
[movement2, probability2] = HistogramFunction(x0,v0,bins, Ahist0, movement1, probability1);
figure(3);
subplot(5,1,2);
plot(movement2(1,:), probability2);
xlabel('Location (timestep 2)'); ylabel('Probability');

%Timestep3
[movement3, probability3] = HistogramFunction(x0,v0,bins, Ahist0,movement2, probability2);
figure(3);
subplot(5,1,3);
plot(movement3(1,:), probability3);
xlabel('Location (timestep 3)'); ylabel('Probability');

%Timestep4
[movement4, probability4] = HistogramFunction(x0,v0,bins, Ahist0,movement3, probability3);
figure(3);
subplot(5,1,4);
plot(movement4(1,:), probability4);
xlabel('Location (timestep 4)'); ylabel('Probability');

%Timestep5
[movement5, probability5] = HistogramFunction(x0,v0,bins, Ahist0,movement4, probability4);
figure(3);
subplot(5,1,5);
plot(movement5(1,:), probability5);
xlabel('Location (timestep 5)'); ylabel('Probability');

%---------------------
%Plot velocities:
figure(4);
subplot(5,1,1);
plot(movement1(2,:), probability1);
xlabel('Velocity (timestep 1)'); ylabel('Probability');

subplot(5,1,2);
plot(movement2(2,:), probability2);
xlabel('Velocity (timestep 2)'); ylabel('Probability');

subplot(5,1,3);
plot(movement3(2,:), probability3);
xlabel('Velocity (timestep 3)'); ylabel('Probability');

subplot(5,1,4);
plot(movement4(2,:), probability4);
xlabel('Velocity (timestep 4)'); ylabel('Probability');

subplot(5,1,5);
plot(movement5(2,:), probability5);
xlabel('Velocity (timestep 5)'); ylabel('Probability');

%--------------------------------------
%Add measurement
[movement6, probability6] = HistogramFunction(x0,v0,zbins, Zhist0,movement1, probability1);
figure(6);
subplot(2,1,1);
plot(movement1(1,:), probability1);
xlabel('Location (timestep 1)'); ylabel('Probability');

subplot(2,1,2);
plot(movement6(1,:), probability6);
xlabel('Location After Update(timestep 1)'); ylabel('Probability');

🎉3 参考文献

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

[1]云晓. 网络移动机器人粒子滤波定位研究[D].天津大学,2006.

[2]刘丽丽,李京华,冯晓毅,石海杰,张晓彪.基于直方图滤波的浅海声源测距算法研究[J].西北工业大学学报,2021,39(03):492-501.

🌈4 Matlab代码实现