keep implementing

Author: AtsushiSakai

Localization/extended_kalman_filter/extended_kalman_filter.py

@@ -10,69 +10,14 @@
 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;
-
-#  % 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');
+#  Simulation parameter
+Qsim = np.diag([0.5, 0.5])**2
+Rsim = np.diag([1.0, math.radians(30.0)])**2
 
-#  DrawGraph(result);
 
 #  function ShowErrorEllipse(xEst,PEst)
 #  %誤差分散円を計算し、表示する関数
@@ -154,35 +99,6 @@
 #  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 @@
 #  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)
+
+        xEst = ekf_estimation(xEst, PEst, ud)
 
-        plt.plot(xTrue[0, 0], xTrue[1, 0], ".r")
+        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)