MyEnigma

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

Unscentedカルマンフィルタを使用した自己位置推定MATLAB, Pythonサンプルプログラム

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


はじめに

以前、自律ロボットにおける

拡張カルマンフィルタを使用した自己位置推定の

MATLAB, Pythonサンプルプログラムを公開しました。

myenigma.hatenablog.com

 

今回は同じくカルマンフィルタの一種である

Unscented Kalman Filter (UKF)

(シグマポイントカルマンフィルタとも呼ばれます)

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


UKFのアルゴリズムの詳しい導出は、

下記のwikiか

カルマンフィルター - Wikipedia

下記の文献を参考にしてください。


MATLABサンプルコード

下記がMATLABサンプルコードです。

% -------------------------------------------------------------------------
%
% File : Unscented KalmanFilterLocalization.m
%
% Discription : Mobible robot localization sample code with
%               Unscented Kalman Filter (UKF)
%
% Environment : Matlab
%
% Author : Atsushi Sakai
%
% Copyright (c): 2014 Atsushi Sakai
%
% License : Modified BSD Software License Agreement
% -------------------------------------------------------------------------
 
function [] = UnscentedKalmanFilterLocalization()
 
close all;
clear all;
 
disp('Unscented Kalman Filter (UKF) 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 v]'
xEst=[0 0 0 0]';
 
% True State
xTrue=xEst;
 
% Dead Reckoning
xd=xTrue;
 
% Observation vector [x y yaw v]'
z=[0 0 0 0]';

% Covariance Matrix for predict
Q=diag([0.1 0.1 toRadian(1) 0.05]).^2;
 
% Covariance Matrix for observation
R=diag([1.5 1.5 toRadian(3) 0.05]).^2;

% Simulation parameter
global Qsigma
Qsigma=diag([0.1 toRadian(20)]).^2;
 
global Rsigma
Rsigma=diag([1.5 1.5 toRadian(3) 0.05]).^2;
 
% UKF Parameter
alpha=0.001;
beta =2;
kappa=0;

n=length(xEst);%size of state vector
lamda=alpha^2*(n+kappa)-n;

%calculate weights
wm=[lamda/(lamda+n)];
wc=[(lamda/(lamda+n))+(1-alpha^2+beta)];
for i=1:2*n
    wm=[wm 1/(2*(n+lamda))];
    wc=[wc 1/(2*(n+lamda))];
end
gamma=sqrt(n+lamda);

PEst = eye(4);
%movcount=0;
tic;
% Main loop
for i=1 : nSteps
    time = time + dt;
    % Input
    u=doControl(time);
    % Observation
    [z,xTrue,xd,u]=Observation(xTrue, xd, u);
    
    % ------ Unscented Kalman Filter --------
    % Predict 
    sigma=GenerateSigmaPoints(xEst,PEst,gamma);
    sigma=PredictMotion(sigma,u);
    xPred=(wm*sigma')';
    PPred=CalcSimgaPointsCovariance(xPred,sigma,wc,Q);
    
    % Update
    y = z - h(xPred);
    sigma=GenerateSigmaPoints(xPred,PPred,gamma);
    zSigma=PredictObservation(sigma);
    zb=(wm*sigma')';
    St=CalcSimgaPointsCovariance(zb,zSigma,wc,R);
    Pxz=CalcPxz(sigma,xPred,zSigma,zb,wc);
    K=Pxz*inv(St);
    xEst = xPred + K*y;
    PEst=PPred-K*St*K';
    
    % Simulation Result
    result.time=[result.time; time];
    result.xTrue=[result.xTrue; xTrue'];
    result.xd=[result.xd; xd'];
    result.xEst=[result.xEst;xEst'];
    result.z=[result.z; z'];
    result.PEst=[result.PEst; diag(PEst)'];
    result.u=[result.u; u'];
    
    %Animation (remove some flames)
    if rem(i,5)==0 
        plot(xTrue(1),xTrue(2),'.b');hold on;
        plot(z(1),z(2),'.g');hold on;
        plot(xd(1),xd(2),'.k');hold on;
        plot(xEst(1),xEst(2),'.r');hold on;
        ShowErrorEllipse(xEst,PEst);
        axis equal;
        grid on;
        drawnow;
        %movcount=movcount+1;
        %mov(movcount) = getframe(gcf);% アニメーションのフレームをゲットする
    end
    
    
end
toc

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



function ShowErrorEllipse(xEst,PEst)
%誤差分散円を計算し、表示する関数
Pxy=PEst(1:2,1:2);%x,yの共分散を取得
[eigvec, eigval]=eig(Pxy);%固有値と固有ベクトルの計算
%固有値の大きい方のインデックスを探す
if eigval(1,1)>=eigval(2,2)
    bigind=1;
    smallind=2;
else
    bigind=2;
    smallind=1;
end

chi=9.21;%誤差楕円のカイの二乗分布値 99%

%楕円描写
t=0:10:360;
a=sqrt(eigval(bigind,bigind)*chi);
b=sqrt(eigval(smallind,smallind)*chi);
x=[a*cosd(t);
   b*sind(t)];
%誤差楕円の角度を計算
angle = atan2(eigvec(bigind,2),eigvec(bigind,1));
if(angle < 0)
    angle = angle + 2*pi;
end

%誤差楕円の回転
R=[cos(angle) sin(angle);
   -sin(angle) cos(angle)];
x=R*x;
plot(x(1,:)+xEst(1),x(2,:)+xEst(2))

function sigma=PredictMotion(sigma,u)
% Sigma Points predition with motion model
for i=1:length(sigma(1,:))
    sigma(:,i)=f(sigma(:,i),u);
end

function sigma=PredictObservation(sigma)
% Sigma Points predition with observation model
for i=1:length(sigma(1,:))
    sigma(:,i)=h(sigma(:,i));
end

function P=CalcSimgaPointsCovariance(xPred,sigma,wc,N)
nSigma=length(sigma(1,:));
d=sigma-repmat(xPred,1,nSigma);
P=N;
for i=1:nSigma
    P=P+wc(i)*d(:,i)*d(:,i)';
end

function P=CalcPxz(sigma,xPred,zSigma,zb,wc)
nSigma=length(sigma(1,:));
dx=sigma-repmat(xPred,1,nSigma);
dz=zSigma-repmat(zb,1,nSigma);
P=zeros(length(sigma(:,1)));
for i=1:nSigma
    P=P+wc(i)*dx(:,i)*dz(:,i)';
end

function sigma=GenerateSigmaPoints(xEst,PEst,gamma)
sigma=xEst;
Psqrt=sqrtm(PEst);
n=length(xEst);
%Positive direction
for ip=1:n
    sigma=[sigma xEst+gamma*Psqrt(:,ip)];
end
%Negative direction
for in=1:n
    sigma=[sigma xEst-gamma*Psqrt(:,in)];
end

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

function z = h(x)
%Observation Model

H = [1 0 0 0
    0 1 0 0
    0 0 1 0
    0 0 0 1 ];
 
z=H*x;

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)
global Qsigma;
global Rsigma;
 
x=f(x, u);% Ground Truth
u=u+Qsigma*randn(2,1);%add Process Noise
xd=f(xd, u);% Dead Reckoning
z=h(x+Rsigma*randn(4,1));%Simulate Observation


function []=DrawGraph(result)
%Plot Result
 
figure(1);
x=[ result.xTrue(:,1:2) result.xEst(:,1:2) result.z(:,1:2)];
set(gca, 'fontsize', 16, 'fontname', 'times');
plot(x(:,5), x(:,6),'.g','linewidth', 4); hold on;
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('UKF Localization Result', 'fontsize', 16, 'fontname', 'times');
xlabel('X (m)', 'fontsize', 16, 'fontname', 'times');
ylabel('Y (m)', 'fontsize', 16, 'fontname', 'times');
legend('Ground Truth','GPS','Dead Reckoning','UKF');
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

Pythonサンプルコード

下記がPythonサンプルコードです。

"""
Uncented kalman filter (UKF) localization sample
author: Atsushi Sakai (@Atsushi_twi)
"""

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

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

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

DT = 0.1  # time tick [s]
SIM_TIME = 50.0  # simulation time [s]

#  UKF Parameter
ALPHA = 0.001
BETA = 2
KAPPA = 0

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):

    xTrue = motion_model(xTrue, u)

    # add noise to gps x-y
    zx = xTrue[0, 0] + np.random.randn() * Qsim[0, 0]
    zy = xTrue[1, 0] + np.random.randn() * Qsim[1, 1]
    z = np.matrix([zx, zy])

    # 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 observation_model(x):
    #  Observation Model
    H = np.matrix([
        [1, 0, 0, 0],
        [0, 1, 0, 0]
    ])

    z = H * x

    return z


def generate_sigma_points(xEst, PEst, gamma):
    sigma = xEst
    Psqrt = np.matrix(scipy.linalg.sqrtm(PEst))
    n = len(xEst[:, 0])
    # Positive direction
    for i in range(n):
        sigma = np.hstack((sigma, xEst + gamma * Psqrt[:, i]))

    # Negative direction
    for i in range(n):
        sigma = np.hstack((sigma, xEst - gamma * Psqrt[:, i]))

    return sigma


def predict_sigma_motion(sigma, u):
    #  Sigma Points predition with motion model
    for i in range(sigma.shape[1]):
        sigma[:, i] = motion_model(sigma[:, i], u)

    return sigma


def predict_sigma_observation(sigma):
    # Sigma Points predition with observation model
    for i in range(sigma.shape[1]):
        sigma[0:2, i] = observation_model(sigma[:, i])

    sigma = sigma[0:2, :]

    return sigma


def calc_sigma_covariance(x, sigma, wc, Pi):
    nSigma = sigma.shape[1]
    d = sigma - x[0:sigma.shape[0], :]
    P = Pi
    for i in range(nSigma):
        P = P + wc[0, i] * d[:, i] * d[:, i].T
    return P


def calc_pxz(sigma, x, z_sigma, zb, wc):
    #  function P=CalcPxz(sigma,xPred,zSigma,zb,wc)
    nSigma = sigma.shape[1]
    dx = np.matrix(sigma - x)
    dz = np.matrix(z_sigma - zb[0:2, :])
    P = np.matrix(np.zeros((dx.shape[0], dz.shape[0])))
    for i in range(nSigma):
        P = P + wc[0, i] * dx[:, i] * dz[:, i].T

    return P


def ukf_estimation(xEst, PEst, z, u, wm, wc, gamma):

    #  Predict
    sigma = generate_sigma_points(xEst, PEst, gamma)
    sigma = predict_sigma_motion(sigma, u)
    xPred = (wm * sigma.T).T
    PPred = calc_sigma_covariance(xPred, sigma, wc, Q)

    #  Update
    zPred = observation_model(xPred)
    y = z.T - zPred
    sigma = generate_sigma_points(xPred, PPred, gamma)
    zb = (wm * sigma.T).T
    z_sigma = predict_sigma_observation(sigma)
    st = calc_sigma_covariance(zb, z_sigma, wc, R)
    Pxz = calc_pxz(sigma, xPred, z_sigma, zb, wc)
    K = Pxz * np.linalg.inv(st)
    xEst = xPred + K * y
    PEst = PPred - K * st * K.T

    return xEst, PEst


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)
    a = math.sqrt(eigval[bigind])
    b = math.sqrt(eigval[smallind])
    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 setup_ukf(nx):
    lamda = ALPHA ** 2 * (nx + KAPPA) - nx
    # calculate weights
    wm = [lamda / (lamda + nx)]
    wc = [(lamda / (lamda + nx)) + (1 - ALPHA ** 2 + BETA)]
    for i in range(2 * nx):
        wm.append(1.0 / (2 * (nx + lamda)))
        wc.append(1.0 / (2 * (nx + lamda)))
    gamma = math.sqrt(nx + lamda)

    wm = np.matrix(wm)
    wc = np.matrix(wc)

    return wm, wc, gamma


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

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

    wm, wc, gamma = setup_ukf(nx)

    # history
    hxEst = xEst
    hxTrue = xTrue
    hxDR = xTrue
    hz = np.zeros((1, 2))

    time = 0.0

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

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

        xEst, PEst = ukf_estimation(xEst, PEst, z, ud, wm, wc, gamma)

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

        if show_animation:
            plt.cla()
            plt.plot(hz[:, 0], hz[:, 1], ".g")
            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()


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

記事冒頭のgif動画のようなシミュレーションが実行されます。

 

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

github.com


UKFの利点と欠点

UKFは、EKFと比べて幾つかの利点と欠点があり、

それらを考慮して利用することが重要です。

利点1: ヤコビ行列の導出が不要

EKFは非線形モデルをヤコビ行列で近似しますが、

モデルが複雑な場合は、ヤコビ行列を導出することが難しいです。

(例えば、レーザのビームモデルを微分することは難しいです.)


一方、UKFでは非線形モデルをUnscented Transform (UT)

というサンプリングベースの近似手法を使用するため、

非常に複雑なモデルでも推定を行うことができます。

利点2: 非線形モデルの近似精度が高い

EKFは非線形モデルをヤコビ行列で近似しています。

ヤコビ行列はテイラー展開の一次項近似ですので、

モデルの非線形性が高いと、

どうしても近似誤差が大きくなってしまいます。


一方、UKFはUTを使用して近似を実施し、

UTはテイラー展開の2~3次項程度の近似精度が

あることがわかっています。


よって、非線形性の高いモデルを使用する場合、

EKFよりもUKFの方が高い推定精度を実現することができます。

欠点1:計算コストが高い

EKFはヤコビ行列を掛け合わせるだけで、

共分散の遷移を実施していますが、

UKFはシグマポイントというサンプリング点を

複数個生成し、それらをモデル遷移して、

重み付き足し算して共分散の遷移を実施します。


なのでEKFと比べると計算コストが高く、

組み込みシステムなどでは、少し問題が生じるかもしれません。

欠点2:調整すべきパラメータの数が多い

EKFはフィルタをチューニングするために、

それぞれの共分散行列のみをチューニングすれば良いですが、

UKFの場合は共分散行列に加えて、

UTの分布を決定するHyper Parameterというパラメータも

チューニングする必要があります。


UKFではα、β、κという3種類のハイパーパラメータが存在しており、

それらを調整する必要があります。


今回のサンプルプログラムでは、

大部分のシステムにおいて、

最適値と言われているパラメータの値を

使用していますが、

当然システムによって最適値は変わるので

チューニングが必要になります。


カルマンフィルタのシステムを開発する際には、

パラメータのチューニングは、大変な作業ですので、

調整すべきパラメータが増えるというのは、

UKFにとって大きな欠点であるといえます。

誤差楕円の計算方法

サンプルコード内の誤差楕円の計算方法に関しては

下記の記事をご覧下さい。

カルマンフィルタにおける誤差楕円の計算方法 - MY ENIGMA

 

MyEnigma Supporters

もしこの記事が参考になり、

ブログをサポートしたいと思われた方は、

こちらからよろしくお願いします。

myenigma.hatenablog.com