MyEnigma

とあるエンジニアのブログです。#Robotics #Programing #C++ #Python #MATLAB #Vim #Mathematics #Book #Movie #Traveling #Mac #iPhone

Particle Filterを使用した自己位置推定MATLAB, Pythonサンプルプログラム

https://github.com/AtsushiSakai/PythonRobotics/raw/master/Localization/particle_filter/animation.gif

はじめに

以前、自律ロボットの位置計測のサンプルプログラムとして、

拡張カルマンフィルタを使用したものと、

myenigma.hatenablog.com

Unscented Kalman Filterを使用したもの、

myenigma.hatenablog.com

Histogram Filterを利用したものを

myenigma.hatenablog.com

紹介しましたが、

今回はもう一つ非常に有名なベイズフィルタの一つである

Particle Filterを使って自己位置推定の概要と、

MATLABとPythonのサンプルプログラムを紹介したいと思います。

パーティクルフィルタとは

パーティクルフィルタの詳細に関しては、

下記のwikiや

粒子フィルタ - Wikipedia

『確率ロボティクス』を読んで頂きたいのですが、

簡単に概要と、利点&欠点を説明したいと思います。


まずはじめににParticle Filterは

ノンパラメトリックなベイズフィルタです。

カルマンフィルタは、ロボットの状態量の誤差分布を

正規分布だと仮定し、その正規分布のパラメータ(平均と標準偏差)

を推定するので、パラメトリックなベイズフィルタと言われます。

(ベイズの定理についてはこちら:

待ち合わせに遅れる彼女、ベイズの定理、そして例題 - MY ENIGMA)


一方、パーティクルフィルタは、そのように誤差分布を仮定せず、

パーティクルと呼ばれるロボットの位置の仮説を複数生成し、

その仮説の分布を元に、ロボットの位置を計算する方法です。


このパーティクルフィルタは、カルマンフィルタと比較すると

いくつか利点と欠点があります。


利点1 確率分布の近似誤差が少ない

EKFやUKFは確率分布をガウス分布で近似しています。

しかし、実際の確率分布はガウス分布で表せない場合が多いため、

おのずと推定誤差が生じてしまいます。


一方、パーティクルフィルタは

複数のサンプリング点とその重みによって

確率分布を表現するため、

複雑な形状の確率分布における推定精度が高いという利点があります。


しかし、注意しなくてはならないことは、

使用するパーティクルの数が無限個あれば、

理論的には確率分布の近似誤差は非常に小さくなりますが、

パーティクルは有限個なので、

少なからず確率分布の近似誤差は発生します。

しかし、十分な数のパーティクルを使用すれば、

多くの場合、それは無視できるぐらい小さくなります。

利点2 複雑なモデルでも利用できる

カルマンフィルタは、

一般的に観測モデルと呼ばれる、

推定する状態ベクトルから、

センサの観測値を計算する線形モデルが必要です。


しかし実際には、

そのような単純な観測モデルは利用できないこともあります。

(グリッドマップを使用したSLAMなど)


そんな時でも、Particle Filterは非常に便利です。

それぞれのパーティクルの尤度さえ計算できれば、

実装できるからです。

利点3 実装が簡単

パーティクルフィルタはカルマンフィルタと比べると

実装が簡単だと言われます。


カルマンフィルタでは

行列の逆行列などを計算しなければなりませんし、

アルゴリズムの内容を理解することは難しい人も多いと思います。


一方、パーティクルフィルタは非常に簡単にコーディングできますし、

アルゴリズムも直感的にわかりやすいものになっています。


欠点1 計算コストが大きい

Particle Filterの一番の欠点は、計算コストが大きいことです。

安定した推定を実現するためには、

多くのパーティクルが必要になります。

従って、カルマンフィルタと比べるとかなり計算コストが大きくなります。


またそれぞれのパーティクルに対して、

Predict, Updateの処理をする必要があるので、

それぞれの処理が複雑な場合、

計算コストが大きくなってしまいます。

例えば、AMCLなどのアルゴリズムでは、

レーザの大量のScanデータに対して、

尤度の計算をしなくてはらないため、

Updateの処理は複雑になります。


しかし、ICPなどのアルゴリズムを組み合わせて、

できるだけ必要なパーティクルの数を減らす手法も提案されています。

ICPアルゴリズムを利用したSLAM用MATLABサンプルプログラム - MY ENIGMA

欠点 2 乱数を使っているため毎回同じ結果がでない

パーティクルフィルタは乱数を使っているため、

なんとなく信頼性に不安があるという意見もあります。


また、同じ観測値のデータを使ってシミュレーションをした場合も、

毎回若干結果が変わるのも問題点としてしばしば指摘されます。

 

MATLABサンプルプログラム

下記がMATLABサンプルプログラムです。

% -------------------------------------------------------------------------
%
% File : ParticleFilterLocalization.m
%
% Discription : Mobible robot localization sample code with
%               ParticleFilterLocalization (PF)
%               The Robot can get a range data from RFID that its position
%               known.
%
% Environment : Matlab
%
% Author : Atsushi Sakai
%
% Copyright (c): 2014 Atsushi Sakai
%
% License : Modified BSD Software License Agreement
% -------------------------------------------------------------------------
 
function [] = ParticleFilterLocalization()
 
close all;
clear all;
 
disp('Particle Filter (PF) sample program start!!')
 
time = 0;
endtime = 60; % [sec]
global dt;
dt = 0.1; % [sec]
 
nSteps = ceil((endtime - time)/dt);
 
result.time=[];
result.xTrue=[];
result.xd=[];
result.xEst=[];
result.z=[];
result.PEst=[];
result.u=[];

% State Vector [x y yaw]'
xEst=[0 0 0]';
 
% True State
xTrue=xEst;
 
% Dead Reckoning Result
xd=xTrue;
 
% Covariance Matrix for predict
Q=diag([0.1 0.1 toRadian(3)]).^2;
 
% Covariance Matrix for observation
R=diag([1]).^2;%[range [m]

% Simulation parameter
global Qsigma
Qsigma=diag([0.1 toRadian(5)]).^2;
 
global Rsigma
Rsigma=diag([0.1]).^2;

%RFIFタグの位置 [x, y]
RFID=[10 0;
      10 10;
      0  15
      -5 20];
  
MAX_RANGE=20;%最大観測距離
NP=100;%パーティクル数
NTh=NP/2.0;%リサンプリングを実施する有効パーティクル数

px=repmat(xEst,1,NP);%パーティクル格納変数
pw=zeros(1,NP)+1/NP;%重み変数
 
tic;
%movcount=0;
% Main loop
for i=1 : nSteps
    time = time + dt;
    % Input
    u=doControl(time);
    % Observation
    [z,xTrue,xd,u]=Observation(xTrue, xd, u, RFID, MAX_RANGE);
    
    % ------ Particle Filter --------
    for ip=1:NP
        x=px(:,ip);
        w=pw(ip);
        
        % Dead Reckoning and random sampling
        x=f(x, u)+sqrt(Q)*randn(3,1);
    
        % Calc Inportance Weight
        for iz=1:length(z(:,1))
            pz=norm(x(1:2)'-z(iz,2:3));
            dz=pz-z(iz,1);
            w=w*Gauss(dz,0,sqrt(R));
        end
        px(:,ip)=x;%格納
        pw(ip)=w;
    end
    
    pw=Normalize(pw,NP);%正規化
    [px,pw]=Resampling(px,pw,NTh,NP);%リサンプリング
    xEst=px*pw';%最終推定値は期待値
    
    % Simulation Result
    result.time=[result.time; time];
    result.xTrue=[result.xTrue; xTrue'];
    result.xd=[result.xd; xd'];
    result.xEst=[result.xEst;xEst'];
    result.u=[result.u; u'];
    
    %Animation (remove some flames)
    if rem(i,5)==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(result.xTrue(:,1),result.xTrue(:,2),'.b');hold on;
        plot(RFID(:,1),RFID(:,2),'pk','MarkerSize',10);hold on;
        %観測線の表示
        if~isempty(z)
            for iz=1:length(z(:,1))
                ray=[xTrue(1:2)';z(iz,2:3)];
                plot(ray(:,1),ray(:,2),'-r');hold on;
            end
        end
        plot(result.xd(:,1),result.xd(:,2),'.k');hold on;
        plot(result.xEst(:,1),result.xEst(:,2),'.r');hold on;
        axis equal;
        grid on;
        drawnow;
        %動画を保存する場合
        %movcount=movcount+1;
        %mov(movcount) = getframe(gcf);% アニメーションのフレームをゲットする
    end
   
end
toc

%アニメーション保存
%movie2avi(mov,'movie.avi');

DrawGraph(result);

function [px,pw]=Resampling(px,pw,NTh,NP)
%リサンプリングを実施する関数
%アルゴリズムはLow Variance Sampling
Neff=1.0/(pw*pw');
if Neff<NTh %リサンプリング
    wcum=cumsum(pw);
    base=cumsum(pw*0+1/NP)-1/NP;%乱数を加える前のbase
    resampleID=base+rand/NP;%ルーレットを乱数分増やす
    ppx=px;%データ格納用
    ind=1;%新しいID
    for ip=1:NP
        while(resampleID(ip)>wcum(ind))
            ind=ind+1;
        end
        px(:,ip)=ppx(:,ind);%LVSで選ばれたパーティクルに置き換え
        pw(ip)=1/NP;%尤度は初期化
    end
end

function pw=Normalize(pw,NP)
%重みベクトルを正規化する関数
sumw=sum(pw);
if sumw~=0
    pw=pw/sum(pw);%正規化
else
    pw=zeros(1,NP)+1/NP;
end
    

function p=Gauss(x,u,sigma)
%ガウス分布の確率密度を計算する関数
p=1/sqrt(2*pi*sigma^2)*exp(-(x-u)^2/(2*sigma^2));

function x = f(x, u)
% Motion Model
 
global dt;
 
F = [1 0 0
    0 1 0
    0 0 1];
 
B = [
    dt*cos(x(3)) 0
    dt*sin(x(3)) 0
    0 dt];
  
x= F*x+B*u;

function u = doControl(time)
%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))]';

%Calc Observation from noise prameter
function [z, x, xd, u] = Observation(x, xd, u, RFID,MAX_RANGE)
global Qsigma;
global Rsigma;
 
x=f(x, u);% Ground Truth
u=u+sqrt(Qsigma)*randn(2,1);%add Process Noise
xd=f(xd, u);% Dead Reckoning
%Simulate Observation
z=[];
for iz=1:length(RFID(:,1))
    d=norm(RFID(iz,:)-x(1:2)');
    if d<MAX_RANGE %観測範囲内
        z=[z;[d+sqrt(Rsigma)*randn(1,1) RFID(iz,:)]];
    end
end

function []=DrawGraph(result)
%Plot Result
 
figure(1);
hold off;
x=[ result.xTrue(:,1:2) result.xEst(:,1:2)];
set(gca, 'fontsize', 16, 'fontname', 'times');
plot(x(:,1), x(:,2),'-.b','linewidth', 4); hold on;
plot(x(:,3), x(:,4),'r','linewidth', 4); hold on;
plot(result.xd(:,1), result.xd(:,2),'--k','linewidth', 4); hold on;
 
title('PF Localization Result', 'fontsize', 16, 'fontname', 'times');
xlabel('X (m)', 'fontsize', 16, 'fontname', 'times');
ylabel('Y (m)', 'fontsize', 16, 'fontname', 'times');
legend('Ground Truth','PF','Dead Reckoning');
grid on;
axis equal;
 
function radian = toRadian(degree)
% degree to radian
radian = degree/180*pi;

function degree = toDegree(radian)
% radian to degree
degree = radian/pi*180;

下記のGitHubリポジトリでも公開しています。

github.com

 

今回のサンプルプログラムは、GPSの観測値ではなく、

設置位置が既知のRFIDからの距離データを元に

位置計測を行うシミュレーションになっています。

RFIDの数や位置、観測範囲は、

プログラムの冒頭で設定されているので、

色々変えてみると楽しいかもしれません。

 

Pythonサンプルプログラム

下記は前述とほぼ同じ、RFIDによる距離情報を使った

Particle filterによる自己位置推定推定Pythonサンプルプログラムです。

"""
Particle Filter localization sample
author: Atsushi Sakai (@Atsushi_twi)
"""

import numpy as np
import math
import matplotlib.pyplot as plt

# Estimation parameter of PF
Q = np.diag([0.1])**2  # range error
R = np.diag([1.0, math.radians(40.0)])**2  # input error

#  Simulation parameter
Qsim = np.diag([0.2])**2
Rsim = np.diag([1.0, math.radians(30.0)])**2

DT = 0.1  # time tick [s]
SIM_TIME = 50.0  # simulation time [s]
MAX_RANGE = 20.0  # maximum observation range

# Particle filter parameter
NP = 100  # Number of Particle
NTh = NP / 2.0  # Number of particle for re-sampling

show_animation = True


def calc_input():
    v = 1.0  # [m/s]
    yawrate = 0.1  # [rad/s]
    u = np.matrix([v, yawrate]).T
    return u


def observation(xTrue, xd, u, RFID):

    xTrue = motion_model(xTrue, u)

    # add noise to gps x-y
    z = np.matrix(np.zeros((0, 3)))

    for i in range(len(RFID[:, 0])):

        dx = xTrue[0, 0] - RFID[i, 0]
        dy = xTrue[1, 0] - RFID[i, 1]
        d = math.sqrt(dx**2 + dy**2)
        if d <= MAX_RANGE:
            dn = d + np.random.randn() * Qsim[0, 0]  # add noise
            zi = np.matrix([dn, RFID[i, 0], RFID[i, 1]])
            z = np.vstack((z, zi))

    # add noise to input
    ud1 = u[0, 0] + np.random.randn() * Rsim[0, 0]
    ud2 = u[1, 0] + np.random.randn() * Rsim[1, 1]
    ud = np.matrix([ud1, ud2]).T

    xd = motion_model(xd, ud)

    return xTrue, z, xd, ud


def motion_model(x, u):

    F = np.matrix([[1.0, 0, 0, 0],
                   [0, 1.0, 0, 0],
                   [0, 0, 1.0, 0],
                   [0, 0, 0, 0]])

    B = np.matrix([[DT * math.cos(x[2, 0]), 0],
                   [DT * math.sin(x[2, 0]), 0],
                   [0.0, DT],
                   [1.0, 0.0]])

    x = F * x + B * u

    return x


def gauss_likelihood(x, sigma):
    p = 1.0 / math.sqrt(2.0 * math.pi * sigma ** 2) * \
        math.exp(-x ** 2 / (2 * sigma ** 2))

    return p


def calc_covariance(xEst, px, pw):
    cov = np.matrix(np.zeros((3, 3)))

    for i in range(px.shape[1]):
        dx = (px[:, i] - xEst)[0:3]
        cov += pw[0, i] * dx * dx.T

    return cov


def pf_localization(px, pw, xEst, PEst, z, u):
    """
    Localization with Particle filter
    """

    for ip in range(NP):
        x = px[:, ip]
        w = pw[0, ip]

        #  Predict with ramdom input sampling
        ud1 = u[0, 0] + np.random.randn() * Rsim[0, 0]
        ud2 = u[1, 0] + np.random.randn() * Rsim[1, 1]
        ud = np.matrix([ud1, ud2]).T
        x = motion_model(x, ud)

        #  Calc Inportance Weight
        for i in range(len(z[:, 0])):
            dx = x[0, 0] - z[i, 1]
            dy = x[1, 0] - z[i, 2]
            prez = math.sqrt(dx**2 + dy**2)
            dz = prez - z[i, 0]
            w = w * gauss_likelihood(dz, math.sqrt(Q[0, 0]))

        px[:, ip] = x
        pw[0, ip] = w

    pw = pw / pw.sum()  # normalize

    xEst = px * pw.T
    PEst = calc_covariance(xEst, px, pw)

    px, pw = resampling(px, pw)

    return xEst, PEst, px, pw


def resampling(px, pw):
    """
    low variance re-sampling
    """

    Neff = 1.0 / (pw * pw.T)[0, 0]  # Effective particle number
    if Neff < NTh:
        wcum = np.cumsum(pw)
        base = np.cumsum(pw * 0.0 + 1 / NP) - 1 / NP
        resampleid = base + np.random.rand(base.shape[1]) / NP

        inds = []
        ind = 0
        for ip in range(NP):
            while resampleid[0, ip] > wcum[0, ind]:
                ind += 1
            inds.append(ind)

        px = px[:, inds]
        pw = np.matrix(np.zeros((1, NP))) + 1.0 / NP  # init weight

    return px, pw


def plot_covariance_ellipse(xEst, PEst):
    Pxy = PEst[0:2, 0:2]
    eigval, eigvec = np.linalg.eig(Pxy)

    if eigval[0] >= eigval[1]:
        bigind = 0
        smallind = 1
    else:
        bigind = 1
        smallind = 0

    t = np.arange(0, 2 * math.pi + 0.1, 0.1)

    #eigval[bigind] or eiqval[smallind] were occassionally negative numbers extremely
    #close to 0 (~10^-20), catch these cases and set the respective variable to 0
    try: a = math.sqrt(eigval[bigind])
    except ValueError: a = 0

    try: b = math.sqrt(eigval[smallind])
    except ValueError: b = 0

    x = [a * math.cos(it) for it in t]
    y = [b * math.sin(it) for it in t]
    angle = math.atan2(eigvec[bigind, 1], eigvec[bigind, 0])
    R = np.matrix([[math.cos(angle), math.sin(angle)],
                   [-math.sin(angle), math.cos(angle)]])
    fx = R * np.matrix([x, y])
    px = np.array(fx[0, :] + xEst[0, 0]).flatten()
    py = np.array(fx[1, :] + xEst[1, 0]).flatten()
    plt.plot(px, py, "--r")


def main():
    print(__file__ + " start!!")

    time = 0.0

    # RFID positions [x, y]
    RFID = np.array([[10.0, 0.0],
                     [10.0, 10.0],
                     [0.0, 15.0],
                     [-5.0, 20.0]])

    # State Vector [x y yaw v]'
    xEst = np.matrix(np.zeros((4, 1)))
    xTrue = np.matrix(np.zeros((4, 1)))
    PEst = np.eye(4)

    px = np.matrix(np.zeros((4, NP)))  # Particle store
    pw = np.matrix(np.zeros((1, NP))) + 1.0 / NP  # Particle weight
    xDR = np.matrix(np.zeros((4, 1)))  # Dead reckoning

    # history
    hxEst = xEst
    hxTrue = xTrue
    hxDR = xTrue

    while SIM_TIME >= time:
        time += DT
        u = calc_input()

        xTrue, z, xDR, ud = observation(xTrue, xDR, u, RFID)

        xEst, PEst, px, pw = pf_localization(px, pw, xEst, PEst, z, ud)

        # store data history
        hxEst = np.hstack((hxEst, xEst))
        hxDR = np.hstack((hxDR, xDR))
        hxTrue = np.hstack((hxTrue, xTrue))

        if show_animation:
            plt.cla()

            for i in range(len(z[:, 0])):
                plt.plot([xTrue[0, 0], z[i, 1]], [xTrue[1, 0], z[i, 2]], "-k")
            plt.plot(RFID[:, 0], RFID[:, 1], "*k")
            plt.plot(px[0, :], px[1, :], ".r")
            plt.plot(np.array(hxTrue[0, :]).flatten(),
                     np.array(hxTrue[1, :]).flatten(), "-b")
            plt.plot(np.array(hxDR[0, :]).flatten(),
                     np.array(hxDR[1, :]).flatten(), "-k")
            plt.plot(np.array(hxEst[0, :]).flatten(),
                     np.array(hxEst[1, :]).flatten(), "-r")
            plot_covariance_ellipse(xEst, PEst)
            plt.axis("equal")
            plt.grid(True)
            plt.pause(0.001)


if __name__ == '__main__':
    main()

下記のGitHubリポジトリでも公開しています。

github.com


上記のコードを実行すると、

記事冒頭のようなシミュレーションが実行されるはずです。


その他のロボティクスアルゴリズムのサンプルコード

こちらを参照下さい。

github.com

github.com