はじめに
以前、自律ロボットにおける
拡張カルマンフィルタを使用した自己位置推定の
MATLAB, Pythonサンプルプログラムを公開しました。
今回は同じくカルマンフィルタの一種である
Unscented Kalman Filter (UKF)
(シグマポイントカルマンフィルタとも呼ばれます)
のMATLABとPythonのサンプルプログラムを公開したいと思います。
UKFのアルゴリズムの詳しい導出は、
下記のwikiか
下記の文献を参考にしてください。
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リポジトリでも公開しています。
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リポジトリでも公開しています。
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にとって大きな欠点であるといえます。