Matlab: 입자 필터 포지셔닝 에뮬레이션
5982 단어 Matlab
function [] = particle_filter_localization()
%PARTICLE_FILTER_LOCALIZATION Summary of this function goes here
% Detailed explanation goes here
% -------------------------------------------------------------------------
% TASK for particle filter localization
% for robotic class in 2018 of ZJU
% Preparartion:
% 1. you need to know how to code and debug in matlab
% 2. understand the theory of Monte Carlo
% Then complete the code by YOURSELF!
% -------------------------------------------------------------------------
close all;
clear all;
disp('Particle Filter program start!!')
%% initialization
tic;
time = 0;
endTime = 60; % second
global dt;
dt = 0.1; % second
nSteps = ceil((endTime - time)/dt);
localizer.time = [];
localizer.xEst = [];
localizer.xGnd = [];
localizer.xOdom = [];
localizer.z = [];
localizer.PEst=[];
localizer.u=[];
% Estimated State [x y yaw]'
xEst=[0 0 0]';
% GroundTruth State
xGnd = xEst;
% Odometry-only = Dead Reckoning
xOdom = xGnd;
% Covariance Matrix for predict
Q=diag([0.1 0.1 toRadian(3)]).^2;
% Covariance Matrix for observation
R=diag([1]).^2;% range:meter
% Simulation parameter
global Qsigma
Qsigma=diag([0.1 toRadian(5)]).^2;
global Rsigma
Rsigma=diag([0.1]).^2;
% landmark position
landMarks=[10 0; 10 10; 0 15; -5 20];
% longest observation confined
MAX_RANGE=20;
% Num of particles, initialized
NP=100;
% Used in Resampling Step, a threshold
NTh=NP/2.0;
% particles produced
px=repmat(xEst,1,NP);
% weights of particles produced
pw=zeros(1,NP)+1/NP;
errs=[];
% sum of error
%% Main Loop
for i=1 : nSteps
time = time + dt;
u=doControl(time);
% do observation
[z,xGnd,xOdom,u]=doObservation(xGnd, xOdom, u, landMarks, MAX_RANGE);
for ip=1:NP
% process every particle
x=px(:,ip);
w=pw(ip);
% do motion model and random sampling
x=doMotion(x, u)+sqrt(Q)*randn(3,1);
% calculate inportance weight
for iz=1:length(z(:,1))
pz=norm(x(1:2)'-z(iz,2:3));
dz=pz-z(iz,1);
w=w*Gaussian(dz,0,sqrt(R));
end
px(:,ip)=x;
pw(ip)=w;
end
pw=Normalization(pw,NP);
[px,pw]=ResamplingStep(px,pw,NTh,NP);
xEst=px*pw';
errs=[errs, norm(xGnd(1:2)'-xEst(1:2)')];
% Simulation Result
localizer.time=[localizer.time; time];
localizer.xGnd=[localizer.xGnd; xGnd'];
localizer.xOdom=[localizer.xOdom; xOdom'];
localizer.xEst=[localizer.xEst;xEst'];
localizer.u=[localizer.u; u'];
% Animation (remove some flames)
if rem(i,10)==0
hold off;
arrow=0.5;
for ip=1:NP
quiver(px(1,ip),px(2,ip),arrow*cos(px(3,ip)),arrow*sin(px(3,ip)),'ok');hold on;
end
plot(localizer.xGnd(:,1),localizer.xGnd(:,2),'.b');hold on;
plot(landMarks(:,1),landMarks(:,2),'pk','MarkerSize',10);hold on;
if~isempty(z)
for iz=1:length(z(:,1))
ray=[xGnd(1:2)';z(iz,2:3)];
plot(ray(:,1),ray(:,2),'-r');hold on;
end
end
plot(localizer.xOdom(:,1),localizer.xOdom(:,2),'.k');hold on;
plot(localizer.xEst(:,1),localizer.xEst(:,2),'.r');hold on;
axis equal;
grid on;
drawnow;
end
end
% draw the final results of localizer, compared to odometry & ground truth
drawResults(localizer);
toc
num2str(sum(errs.^2))
figure(2)
plot(0.1:0.1:60,errs)
xlabel("time")
ylabel("error square")
end
%% Other functions
% degree to radian
function radian = toRadian(degree)
radian = degree/180*pi;
end
function []=drawResults(localizer)
%Plot Result
figure(1);
hold off;
x=[ localizer.xGnd(:,1:2) localizer.xEst(:,1:2)];
set(gca, 'fontsize', 12, 'fontname', 'times');
plot(x(:,1), x(:,2),'-.b','linewidth', 4); hold on;
plot(x(:,3), x(:,4),'r','linewidth', 4); hold on;
plot(localizer.xOdom(:,1), localizer.xOdom(:,2),'--k','linewidth', 4); hold on;
title('Localization Result', 'fontsize', 12, 'fontname', 'times');
xlabel('X (m)', 'fontsize', 12, 'fontname', 'times');
ylabel('Y (m)', 'fontsize', 12, 'fontname', 'times');
legend('Ground Truth','Particle Filter','Odometry Only');
grid on;
axis equal;
end
function [ u ] = doControl( time )
%DOCONTROL Summary of this function goes here
% Detailed explanation goes here
%Calc Input Parameter
T=10; % [sec]
% [V yawrate]
V=1.0; % [m/s]
yawrate = 5; % [deg/s]
u =[ V*(1-exp(-time/T)) toRadian(yawrate)*(1-exp(-time/T))]';
end
%% you need to complete
% do Observation model
function [z, xGnd, xOdom, u] = doObservation(xGnd, xOdom, u, landMarks, MAX_RANGE)
global Qsigma;
global Rsigma;
% Gnd Truth and Odometry
xGnd=doMotion(xGnd, u);% Ground Truth
u=u+sqrt(Qsigma)*randn(2,1); % add noise randomly
xOdom=doMotion(xOdom, u); % odometry only
%Simulate Observation
z=[];
for iz=1:length(landMarks(:,1))
% d = ?
d = norm(xGnd(1:2)'-landMarks(iz,:));
if d= u
px_(:,i) = px(:,j);
break;
end
end
end
px = px_;
pw=zeros(1,NP)+1/NP;
end
end
이 내용에 흥미가 있습니까?
현재 기사가 여러분의 문제를 해결하지 못하는 경우 AI 엔진은 머신러닝 분석(스마트 모델이 방금 만들어져 부정확한 경우가 있을 수 있음)을 통해 가장 유사한 기사를 추천합니다:
[matlab] 주파수 필터 함수 lpfilt ()텍스트를 자유롭게 공유하거나 복사할 수 있습니다.하지만 이 문서의 URL은 참조 URL로 남겨 두십시오.
CC BY-SA 2.5, CC BY-SA 3.0 및 CC BY-SA 4.0에 따라 라이센스가 부여됩니다.