From bf093a95a3df2f757fae3a907be525c6fde36c36 Mon Sep 17 00:00:00 2001 From: Yo Iida Date: Wed, 7 Nov 2018 11:41:21 +0900 Subject: [PATCH] fix EKF --- .../extended_kalman_filter/extended_kalman_filter.py | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/Localization/extended_kalman_filter/extended_kalman_filter.py b/Localization/extended_kalman_filter/extended_kalman_filter.py index e6089441..93a6c9d0 100644 --- a/Localization/extended_kalman_filter/extended_kalman_filter.py +++ b/Localization/extended_kalman_filter/extended_kalman_filter.py @@ -11,8 +11,8 @@ import math import matplotlib.pyplot as plt # Estimation parameter of EKF -Q = np.diag([0.1, 0.1, np.deg2rad(1.0), 1.0])**2 -R = np.diag([1.0, np.deg2rad(40.0)])**2 +Q = np.diag([1.0, np.deg2rad(40.0)])**2 +R = np.diag([0.1, 0.1, np.deg2rad(1.0), 1.0])**2 # Simulation parameter Qsim = np.diag([0.5, 0.5])**2 @@ -120,13 +120,13 @@ def ekf_estimation(xEst, PEst, z, u): # Predict xPred = motion_model(xEst, u) jF = jacobF(xPred, u) - PPred = jF * PEst * jF.T + Q + PPred = jF.dot(PEst).dot(jF.T) + R # Update jH = jacobH(xPred) zPred = observation_model(xPred) y = z.T - zPred - S = jH.dot(PPred).dot(jH.T) + R + S = jH.dot(PPred).dot(jH.T) + Q K = PPred.dot(jH.T).dot(np.linalg.inv(S)) xEst = xPred + K.dot(y) PEst = (np.eye(len(xEst)) - K.dot(jH)).dot(PPred) @@ -183,7 +183,7 @@ def main(): xTrue, z, xDR, ud = observation(xTrue, xDR, u) - xEst, PEst = ekf_estimation(xEst, PEst, z, ud) + xEst, PEst = ekf_estimation(xEst, PEst, z, u) # store data history hxEst = np.hstack((hxEst, xEst))