diff --git a/Localization/extended_kalman_filter/extended_kalman_filter.py b/Localization/extended_kalman_filter/extended_kalman_filter.py index b0e40b06..91d358c4 100644 --- a/Localization/extended_kalman_filter/extended_kalman_filter.py +++ b/Localization/extended_kalman_filter/extended_kalman_filter.py @@ -10,69 +10,14 @@ import numpy as np import math import matplotlib.pyplot as plt +# Estimation parameter +Q = np.diag([0.5, 0.5])**2 +R = np.diag([1.0, math.radians(30.0)])**2 -# Covariance Matrix for motion -# Q=diag([0.1 0.1 toRadian(1) 0.05]).^2; +# Simulation parameter +Qsim = np.diag([0.5, 0.5])**2 +Rsim = np.diag([1.0, math.radians(30.0)])**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; %[v yawrate] - -# global Rsigma -# Rsigma=diag([1.5 1.5 toRadian(3) 0.05]).^2;%[x y z yaw v] - -# PEst = eye(4); - -# u=doControl(time); -# % Observation -# [z,xTrue,xd,u]=Observation(xTrue, xd, u); - -# % ------ Kalman Filter -------- -# % Predict -# xPred = f(xEst, u); -# F=jacobF(xPred, u); -# PPred= F*PEst*F' + Q; - -# % Update -# H=jacobH(xPred); -# y = z - h(xPred); -# S = H*PPred*H' + R; -# K = PPred*H'*inv(S); -# xEst = xPred + K*y; -# PEst = (eye(size(xEst,1)) - K*H)*PPred; - -# % 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 -# %hold off; -# plot(result.xTrue(:,1),result.xTrue(:,2),'.b');hold on; -# plot(result.z(:,1),result.z(:,2),'.g');hold on; -# plot(result.xd(:,1),result.xd(:,2),'.k');hold on; -# plot(result.xEst(:,1),result.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) # %誤差分散円を計算し、表示する関数 @@ -154,35 +99,6 @@ import matplotlib.pyplot as plt # 0 0 0 1]; -# function [z, x, xd, u] = Observation(x, xd, u) -# %Calc Observation from noise prameter -# 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('EKF 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','EKF','Error Ellipse'); -# grid on; -# axis equal; - # function angle=Pi2Pi(angle) # %ロボットの角度を-pi~piの範囲に補正する関数 # angle = mod(angle, 2*pi); @@ -194,14 +110,6 @@ import matplotlib.pyplot as plt # angle(i) = angle(i) + 2*pi; -# function radian = toRadian(degree) -# % degree to radian -# radian = degree/180*pi; - -# function degree = toDegree(radian) -# % radian to degree -# degree = radian/pi*180; - DT = 0.1 # time tick [s] SIM_TIME = 60.0 # simulation time [s] @@ -215,12 +123,21 @@ def do_control(): return u -# z, xTrue, xd, u = Observation(xTrue, xd, u) def observation(xTrue, xd, u): xTrue = motion_model(xTrue, u) - return xTrue + 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]) + + 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): @@ -240,29 +157,49 @@ def motion_model(x, u): return x +def ekf_estimation(xEst, PEst, u): + + # Predict + xPred = motion_model(xEst, u) + # F=jacobF(xPred, u); + # PPred= F*PEst*F' + Q; + + # Update + # H=jacobH(xPred); + # y = z - h(xPred); + # S = H*PPred*H' + R; + # K = PPred*H'*inv(S); + # xEst = xPred + K*y; + xEst = xPred + # PEst = (eye(size(xEst,1)) - K*H)*PPred; + + return xEst + + def main(): print(__file__ + " start!!") time = 0.0 # State Vector [x y yaw v]' - # xEst = np.matrix(np.zeros((3, 1))) + xEst = np.matrix(np.zeros((4, 1))) xTrue = np.matrix(np.zeros((4, 1))) + PEst = np.eye(4) # Dead Reckoning xDR = np.matrix(np.zeros((4, 1))) - # Observation vector - # z = np.matrix(np.zeros((2, 1))) - while SIM_TIME >= time: - # print(time) time += DT - u = do_control() - xTrue = observation(xTrue, xDR, u) + xTrue, z, xDR, ud = observation(xTrue, xDR, u) - plt.plot(xTrue[0, 0], xTrue[1, 0], ".r") + xEst = ekf_estimation(xEst, PEst, ud) + + plt.plot(xTrue[0, 0], xTrue[1, 0], ".b") + plt.plot(xDR[0, 0], xDR[1, 0], ".k") + plt.plot(z[0, 0], z[0, 1], ".g") + plt.plot(xEst[0, 0], xEst[1, 0], ".r") plt.axis("equal") plt.grid(True) plt.pause(0.001)