Addition of velocity scale factor estimation to EKF

Localization/extended_kalman_filter/extended_kalman_filter.py

@@ -21,13 +21,14 @@
     0.1,  # variance of location on x-axis
     0.1,  # variance of location on y-axis
     np.deg2rad(1.0),  # variance of yaw angle
-    1.0  # variance of velocity
+    0.4,  # variance of velocity
+    0.1  # variance of scale factor
 ]) ** 2  # predict state covariance
-R = np.diag([1.0, 1.0]) ** 2  # Observation x,y position covariance
+R = np.diag([0.1, 0.1]) ** 2  # Observation x,y position covariance
 
 #  Simulation parameter
-INPUT_NOISE = np.diag([1.0, np.deg2rad(30.0)]) ** 2
-GPS_NOISE = np.diag([0.5, 0.5]) ** 2
+INPUT_NOISE = np.diag([0.1, np.deg2rad(5.0)]) ** 2
+GPS_NOISE = np.diag([0.05, 0.05]) ** 2
 
 DT = 0.1  # time tick [s]
 SIM_TIME = 50.0  # simulation time [s]
@@ -57,27 +58,26 @@ def observation(xTrue, xd, u):
 
 
 def motion_model(x, u):
-    F = np.array([[1.0, 0, 0, 0],
-                  [0, 1.0, 0, 0],
-                  [0, 0, 1.0, 0],
-                  [0, 0, 0, 0]])
-
-    B = np.array([[DT * math.cos(x[2, 0]), 0],
-                  [DT * math.sin(x[2, 0]), 0],
+    F = np.array([[1.0, 0, 0, 0, 0],
+                  [0, 1.0, 0, 0, 0],
+                  [0, 0, 1.0, 0, 0],
+                  [0, 0, 0, 0, 0],
+                  [0, 0, 0, 0, 1.0]])
+
+    B = np.array([[DT * math.cos(x[2, 0]) * x[4, 0], 0],
+                  [DT * math.sin(x[2, 0]) * x[4, 0], 0],
                   [0.0, DT],
-                  [1.0, 0.0]])
+                  [1.0, 0.0],
+                  [0.0, 0.0]])
 
     x = F @ x + B @ u
 
     return x
 
 
 def observation_model(x):
-    H = np.array([
-        [1, 0, 0, 0],
-        [0, 1, 0, 0]
-    ])
-
+    H = np.array([[1, 0, 0, 0, 0],
+                  [0, 1, 0, 0, 0]])
     z = H @ x
 
     return z
@@ -88,34 +88,33 @@ def jacob_f(x, u):
     Jacobian of Motion Model
 
     motion model
-    x_{t+1} = x_t+v*dt*cos(yaw)
-    y_{t+1} = y_t+v*dt*sin(yaw)
+    x_{t+1} = x_t+v*s*dt*cos(yaw)
+    y_{t+1} = y_t+v*s*dt*sin(yaw)
     yaw_{t+1} = yaw_t+omega*dt
     v_{t+1} = v{t}
     so
     dx/dyaw = -v*dt*sin(yaw)
     dx/dv = dt*cos(yaw)
     dy/dyaw = v*dt*cos(yaw)
     dy/dv = dt*sin(yaw)
+    dx/ds = dt*v*cos(yaw)
+    dy/ds = dt*v*sin(yaw)
     """
     yaw = x[2, 0]
     v = u[0, 0]
+    s = x[4, 0]
     jF = np.array([
-        [1.0, 0.0, -DT * v * math.sin(yaw), DT * math.cos(yaw)],
-        [0.0, 1.0, DT * v * math.cos(yaw), DT * math.sin(yaw)],
-        [0.0, 0.0, 1.0, 0.0],
-        [0.0, 0.0, 0.0, 1.0]])
-
+        [1.0, 0.0, -DT * v * s * math.sin(yaw), DT * s * math.cos(yaw), DT * v * math.cos(yaw)],
+        [0.0, 1.0, DT * v * s * math.cos(yaw), DT * s * math.sin(yaw), DT * v * math.sin(yaw)],
+        [0.0, 0.0, 1.0, 0.0, 0.0],
+        [0.0, 0.0, 0.0, 1.0, 0.0],
+        [0.0, 0.0, 0.0, 0.0, 1.0]])
     return jF
 
 
 def jacob_h():
-    # Jacobian of Observation Model
-    jH = np.array([
-        [1, 0, 0, 0],
-        [0, 1, 0, 0]
-    ])
-
+    jH = np.array([[1, 0, 0, 0, 0],
+                   [0, 1, 0, 0, 0]])
     return jH
 
 
@@ -141,12 +140,15 @@ def main():
 
     time = 0.0
 
-    # State Vector [x y yaw v]'
-    xEst = np.zeros((4, 1))
-    xTrue = np.zeros((4, 1))
-    PEst = np.eye(4)
+    #  State Vector [x y yaw v s]'
+    xEst = np.zeros((5, 1))
+    xEst[4, 0] = 1.0 #  Initial scale factor
+    xTrue = np.zeros((5, 1))
+    true_scale_factor = 0.9 #  True scale factor
+    xTrue[4, 0] = true_scale_factor
+    PEst = np.eye(5)
 
-    xDR = np.zeros((4, 1))  # Dead reckoning
+    xDR = np.zeros((5, 1))  # Dead reckoning
 
     # history
     hxEst = xEst
@@ -167,7 +169,7 @@ def main():
         hxDR = np.hstack((hxDR, xDR))
         hxTrue = np.hstack((hxTrue, xTrue))
         hz = np.hstack((hz, z))
-
+        estimated_scale_factor = hxEst[4, -1]
         if show_animation:
             plt.cla()
             # for stopping simulation with the esc key.
@@ -180,6 +182,8 @@ def main():
                      hxDR[1, :].flatten(), "-k")
             plt.plot(hxEst[0, :].flatten(),
                      hxEst[1, :].flatten(), "-r")
+            plt.text(0.45, 0.85, f"True Velocity Scale Factor: {true_scale_factor:.2f}", ha='left', va='top', transform=plt.gca().transAxes)
+            plt.text(0.45, 0.95, f"Estimated Velocity Scale Factor: {estimated_scale_factor:.2f}", ha='left', va='top', transform=plt.gca().transAxes)
             plot_covariance_ellipse(xEst[0, 0], xEst[1, 0], PEst)
             plt.axis("equal")
             plt.grid(True)