How to tune extended kalman filter on PyKalman?
Question:
I would like to correct some predicted vehicular variables: latitude, longitude, speed, heading and acceleration. Inside the following code, there are the values of those variables. There are 20 samples, the first 10 samples are the original data and the last 10 samples the predicted ones. I expected EKF to denoise those predicted values, however, the computed values are even far from the original.
How could I tune the code to get realistic computed values? Otherwise, are there techniques to correct predicted vehicular coordinates?
import numpy as np
np.set_printoptions(precision=6, suppress=True)
A is the identity matrix.
# A is sometimes F in the literature.
A_k_minus_1 = np.array([[1.0, 0, 0, 0, 0],
[0, 1.0, 0, 0, 0],
[0, 0, 1.0, 0, 0],
[0, 0, 0, 1.0, 0],
[0, 0, 0, 0, 1.0]])
# This is a vector with the number of elements equal to the number of states
process_noise_v_k_minus_1 = np.array([0.01, 0.01, 0.01, 0.01, 0.01])
# State model noise covariance matrix Q_k
# When Q is large, the Kalman Filter tracks large changes in
# the sensor measurements more closely than for smaller Q.
# Q is a square matrix that has the same number of rows as states.
Q_k = np.array([[1.0, 0, 0, 0, 0],
[0, 1.0, 0, 0, 0],
[0, 0, 1.0, 0, 0],
[0, 0, 0, 1.0, 0],
[0, 0, 0, 0, 1.0]])
# Measurement matrix H_k
# Used to convert the predicted state estimate at time k
# into predicted sensor measurements at time k.
# In this case, H will be the identity matrix since the
# estimated state maps directly to state measurements data
# H has the same number of rows as sensor measurements
# and same number of columns as states.
H_k = np.array([[1.0, 0, 0, 0, 0],
[0, 1.0, 0, 0, 0],
[0, 0, 1.0, 0, 0],
[0, 0, 0, 1.0, 0],
[0, 0, 0, 0, 1.0]])
# Sensor measurement noise covariance matrix R_k
# Has the same number of rows and columns as sensor measurements.
# If we are sure about the measurements, R will be near zero.
R_k = np.array([[1.0, 0, 0, 0, 0],
[0, 1.0, 0, 0, 0],
[0, 0, 1.0, 0, 0],
[0, 0, 0, 1.0, 0],
[0, 0, 0, 0, 1.0]])
# Sensor noise. This is a vector with the
# number of elements equal to the number of sensor measurements.
sensor_noise_w_k = np.array([0.01, 0.01, 0.01, 0.01, 0.01])
def ekf(z_k_observation_vector, state_estimate_k_minus_1,
control_vector_k_minus_1, P_k_minus_1, dk):
######################### Predict #############################
# Predict the state estimate at time k based on the state
# estimate at time k-1 and the control input applied at time k-1.
state_estimate_k = A_k_minus_1 @ (
state_estimate_k_minus_1) + (
control_vector_k_minus_1) + (
process_noise_v_k_minus_1)
print(f'State Estimate Before EKF={state_estimate_k}')
# Predict the state covariance estimate based on the previous
# covariance and some noise
P_k = A_k_minus_1 @ P_k_minus_1 @ A_k_minus_1.T + (
Q_k)
################### Update (Correct) ##########################
# Calculate the difference between the actual sensor measurements
# at time k minus what the measurement model predicted
# the sensor measurements would be for the current timestep k.
measurement_residual_y_k = z_k_observation_vector - (
(H_k @ state_estimate_k) + (sensor_noise_w_k))
print(f'Observation={z_k_observation_vector}')
# Calculate the measurement residual covariance
S_k = H_k @ P_k @ H_k.T + R_k
# Calculate the near-optimal Kalman gain
# We use pseudoinverse since some of the matrices might be
# non-square or singular.
K_k = P_k @ H_k.T @ np.linalg.pinv(S_k)
# Calculate an updated state estimate for time k
state_estimate_k = state_estimate_k + (K_k @ measurement_residual_y_k)
# Update the state covariance estimate for time k
P_k = P_k - (K_k @ H_k @ P_k)
# Print the best (near-optimal) estimate of the current state of the robot
print(f'State Estimate After EKF={state_estimate_k}')
# Return the updated state and covariance estimates
return state_estimate_k, P_k
def main():
# We start at time k=1
k = 1
# Time interval in seconds
dk = 1
# Create a list of sensor observations at successive timesteps
# Each list within z_k is an observation vector.
z_k = np.array([[41.3920877, 2.165009483, 11.32350106, 190.255528, 1.05357508],
[41.39198647, 2.16505734, 12.49313626, 148.3052463, 1.169635197],
[41.3919087, 2.165148001, 11.48702769, 139.2730885, -1.006108567],
[41.39181576, 2.165256339, 13.72677596, 139.2730885, 2.239748268],
[41.39173316, 2.165357368, 12.46676741, 137.5553644, -1.260008547],
[41.39166792, 2.165438147, 9.901313858, 137.5553644, -2.565453555],
[41.39162805, 2.165487467, 6.047907658, 137.5827278, -3.8534062],
[41.39161926, 2.165498327, 1.332407586, 137.5954214, -4.715500072],
[41.39161926, 2.165498327, 0, 137.5954214, -1.332407586],
[41.39161926, 2.165498327, 0, 137.5954214, 0],
[41.391619, 2.165498, -1.930117, 137.595421, 1.023591],
[41.391619, 2.165498, -1.710974, 137.595421, 2.246764],
[41.391619, 2.165498, - 0.375267, 137.595421, 3.334014],
[41.391619, 2.165498, 2.287329, 137.595421, 4.192694],
[41.391619, 2.165498, 6.132224, 137.595421, 4.717386],
[41.391619, 2.165498, 7.416489, 137.595421, 4.833027],
[41.391619, 2.165498, 7.555815, 137.595421, 4.833027],
[41.391619, 2.165498, 9.685403, 137.595421, 4.833027],
[41.391619, 2.165498, 9.696774, 137.595421, 4.833027],
[41.391619, 2.165498, 8.403750, 137.595421, 4.833027]])
# The estimated state vector at time k-1 in the global reference frame.
# [x_k_minus_1, y_k_minus_1, yaw_k_minus_1]
# [meters, meters, radians]
state_estimate_k_minus_1 = np.array([0.0, 0.0, 0.0, 0.0, 0.0])
# The control input vector at time k-1 in the global reference frame.
# [v, yaw_rate]
# [meters/second, radians/second]
# In the literature, this is commonly u.
# Because there is no angular velocity and the robot begins at the
# origin with a 0 radians yaw angle, this robot is traveling along
# the positive x-axis in the global reference frame.
control_vector_k_minus_1 = np.array([41, 2, 0.0, 0.0, 0.0])
# State covariance matrix P_k_minus_1
# This matrix has the same number of rows (and columns) as the
# number of states. P is sometimes referred
# to as Sigma in the literature. It represents an estimate of
# the accuracy of the state estimate at time k made using the
# state transition matrix. We start off with guessed values.
P_k_minus_1 = np.array([[1.0, 0, 0, 0, 0],
[0, 1.0, 0, 0, 0],
[0, 0, 1.0, 0, 0],
[0, 0, 0, 1.0, 0],
[0, 0, 0, 0, 1.0]])
# Start at k=1 and go through each of the 20 sensor observations,
# one at a time.
# We stop right after timestep k=20 (i.e. the last sensor observation)
for k, obs_vector_z_k in enumerate(z_k, start=1):
# Print the current timestep
print(f'Timestep k={k}')
# Run the Extended Kalman Filter and store the
# near-optimal state and covariance estimates
optimal_state_estimate_k, covariance_estimate_k = ekf(
obs_vector_z_k, # Most recent sensor measurement
state_estimate_k_minus_1, # Our most recent estimate of the state
control_vector_k_minus_1, # Our most recent control input
P_k_minus_1, # Our most recent state covariance matrix
dk) # Time interval
# Get ready for the next timestep by updating the variable values
state_estimate_k_minus_1 = optimal_state_estimate_k
P_k_minus_1 = covariance_estimate_k
# Print a blank line
print()
# Program starts running here with the main method
main()
The output is:
Timestep k=1
State Estimate Before EKF=[41.01 2.01 0.01 0.01 0.01]
[ 41.392088 2.165009 11.323501 190.255528 1.053575]
Observation=[ 41.392088 2.165009 11.323501 190.255528 1.053575]
State Estimate After EKF=[ 41.258058 2.106673 7.545667 126.833685 0.69905 ]
Timestep k=2
State Estimate Before EKF=[ 82.268058 4.116673 7.555667 126.843685 0.70905 ]
[ 41.391986 2.165057 12.493136 148.305246 1.169635]
Observation=[ 41.391986 2.165057 12.493136 148.305246 1.169635]
State Estimate After EKF=[ 56.714263 2.890663 10.635335 140.250911 0.990666]
Timestep k=3
State Estimate Before EKF=[ 97.724263 4.900663 10.645335 140.260911 1.000666]
[ 41.391909 2.165148 11.487028 139.273089 -1.006109]
Observation=[ 41.391909 2.165148 11.487028 139.273089 -1.006109]
State Estimate After EKF=[ 62.845663 3.201059 11.160193 139.643211 -0.247814]
Timestep k=4
State Estimate Before EKF=[103.855663 5.211059 11.170193 139.653211 -0.237814]
[ 41.391816 2.165256 13.726776 139.273089 2.239748]
Observation=[ 41.391816 2.165256 13.726776 139.273089 2.239748]
State Estimate After EKF=[ 65.235466 3.322017 12.744444 139.412044 1.287588]
Timestep k=5
State Estimate Before EKF=[106.245466 5.332017 12.754444 139.422044 1.297588]
[ 41.391733 2.165357 12.466767 137.555364 -1.260009]
Observation=[ 41.391733 2.165357 12.466767 137.555364 -1.260009]
State Estimate After EKF=[ 66.156076 3.368665 12.570463 138.262152 -0.289329]
Timestep k=6
State Estimate Before EKF=[107.166076 5.378665 12.580463 138.272152 -0.279329]
[ 41.391668 2.165438 9.901314 137.555364 -2.565454]
Observation=[ 41.391668 2.165438 9.901314 137.555364 -2.565454]
State Estimate After EKF=[ 66.508869 3.386591 10.918469 137.82297 -1.698419]
Timestep k=7
State Estimate Before EKF=[107.518869 5.396591 10.928469 137.83297 -1.688419]
[ 41.391628 2.165487 6.047908 137.582728 -3.853406]
Observation=[ 41.391628 2.165487 6.047908 137.582728 -3.853406]
State Estimate After EKF=[ 66.643776 3.393477 7.905934 137.672131 -3.032636]
Timestep k=8
State Estimate Before EKF=[107.653776 5.403477 7.915934 137.682131 -3.022636]
[ 41.391619 2.165498 1.332408 137.595421 -4.7155 ]
Observation=[ 41.391619 2.165498 1.332408 137.595421 -4.7155 ]
State Estimate After EKF=[ 66.695326 3.396116 3.84091 137.622361 -4.075064]
Timestep k=9
State Estimate Before EKF=[107.705326 5.406116 3.85091 137.632361 -4.065064]
[ 41.391619 2.165498 0. 137.595421 -1.332408]
Observation=[ 41.391619 2.165498 0. 137.595421 -1.332408]
State Estimate After EKF=[ 66.71502 3.397124 1.464736 137.603351 -2.38237 ]
Timestep k=10
State Estimate Before EKF=[107.72502 5.407124 1.474736 137.613351 -2.37237 ]
[ 41.391619 2.165498 0. 137.595421 0. ]
Observation=[ 41.391619 2.165498 0. 137.595421 0. ]
State Estimate After EKF=[ 66.722543 3.397509 0.557119 137.59609 -0.912345]
Timestep k=11
State Estimate Before EKF=[107.732543 5.407509 0.567119 137.60609 -0.902345]
[ 41.391619 2.165498 -1.930117 137.595421 1.023591]
Observation=[ 41.391619 2.165498 -1.930117 137.595421 1.023591]
State Estimate After EKF=[ 66.725417 3.397656 -0.982438 137.593316 0.281769]
Timestep k=12
State Estimate Before EKF=[107.735417 5.407656 -0.972438 137.603316 0.291769]
[ 41.391619 2.165498 -1.710974 137.595421 2.246764]
Observation=[ 41.391619 2.165498 -1.710974 137.595421 2.246764]
State Estimate After EKF=[ 66.726515 3.397712 -1.435059 137.592256 1.493842]
Timestep k=13
State Estimate Before EKF=[107.736515 5.407712 -1.425059 137.602256 1.503842]
[ 41.391619 2.165498 -0.375267 137.595421 3.334014]
Observation=[ 41.391619 2.165498 -0.375267 137.595421 3.334014]
State Estimate After EKF=[ 66.726934 3.397733 -0.782432 137.591851 2.62877 ]
Timestep k=14
State Estimate Before EKF=[107.736934 5.407733 -0.772432 137.601851 2.63877 ]
[ 41.391619 2.165498 2.287329 137.595421 4.192694]
Observation=[ 41.391619 2.165498 2.287329 137.595421 4.192694]
State Estimate After EKF=[ 66.727094 3.397741 1.112424 137.591697 3.592968]
Timestep k=15
State Estimate Before EKF=[107.737094 5.407741 1.122424 137.601697 3.602968]
[ 41.391619 2.165498 6.132224 137.595421 4.717386]
Observation=[ 41.391619 2.165498 6.132224 137.595421 4.717386]
State Estimate After EKF=[ 66.727155 3.397744 4.21247 137.591638 4.285536]
Timestep k=16
State Estimate Before EKF=[107.737155 5.407744 4.22247 137.601638 4.295536]
[ 41.391619 2.165498 7.416489 137.595421 4.833027]
Observation=[ 41.391619 2.165498 7.416489 137.595421 4.833027]
State Estimate After EKF=[ 66.727178 3.397746 6.190302 137.591615 4.621543]
Timestep k=17
State Estimate Before EKF=[107.737178 5.407746 6.200302 137.601615 4.631543]
[ 41.391619 2.165498 7.555815 137.595421 4.833027]
Observation=[ 41.391619 2.165498 7.555815 137.595421 4.833027]
State Estimate After EKF=[ 66.727187 3.397746 7.031875 137.591607 4.749887]
Timestep k=18
State Estimate Before EKF=[107.737187 5.407746 7.041875 137.601607 4.759887]
[ 41.391619 2.165498 9.685403 137.595421 4.833027]
Observation=[ 41.391619 2.165498 9.685403 137.595421 4.833027]
State Estimate After EKF=[ 66.727191 3.397746 8.669485 137.591603 4.79891 ]
Timestep k=19
State Estimate Before EKF=[107.737191 5.407746 8.679485 137.601603 4.80891 ]
[ 41.391619 2.165498 9.696774 137.595421 4.833027]
Observation=[ 41.391619 2.165498 9.696774 137.595421 4.833027]
State Estimate After EKF=[ 66.727192 3.397746 9.302024 137.591602 4.817635]
Timestep k=20
State Estimate Before EKF=[107.737192 5.407746 9.312024 137.601602 4.827635]
[ 41.391619 2.165498 8.40375 137.595421 4.833027]
Observation=[ 41.391619 2.165498 8.40375 137.595421 4.833027]
State Estimate After EKF=[ 66.727193 3.397746 8.744499 137.591602 4.824787]
Answers:
Actually, your Kalman is linear, not extended Kalman Filter.
Your problem is in the function ekf when you define the variable state_estimate_k.
The formula should be x(estimate) = F*x + B*u. Just change your code to this :
state_estimate_k = A_k_minus_1 @ (
state_estimate_k_minus_1) + (
control_vector_k_minus_1) @ (
process_noise_v_k_minus_1)
I would like to correct some predicted vehicular variables: latitude, longitude, speed, heading and acceleration. Inside the following code, there are the values of those variables. There are 20 samples, the first 10 samples are the original data and the last 10 samples the predicted ones. I expected EKF to denoise those predicted values, however, the computed values are even far from the original.
How could I tune the code to get realistic computed values? Otherwise, are there techniques to correct predicted vehicular coordinates?
import numpy as np
np.set_printoptions(precision=6, suppress=True)
A is the identity matrix.
# A is sometimes F in the literature.
A_k_minus_1 = np.array([[1.0, 0, 0, 0, 0],
[0, 1.0, 0, 0, 0],
[0, 0, 1.0, 0, 0],
[0, 0, 0, 1.0, 0],
[0, 0, 0, 0, 1.0]])
# This is a vector with the number of elements equal to the number of states
process_noise_v_k_minus_1 = np.array([0.01, 0.01, 0.01, 0.01, 0.01])
# State model noise covariance matrix Q_k
# When Q is large, the Kalman Filter tracks large changes in
# the sensor measurements more closely than for smaller Q.
# Q is a square matrix that has the same number of rows as states.
Q_k = np.array([[1.0, 0, 0, 0, 0],
[0, 1.0, 0, 0, 0],
[0, 0, 1.0, 0, 0],
[0, 0, 0, 1.0, 0],
[0, 0, 0, 0, 1.0]])
# Measurement matrix H_k
# Used to convert the predicted state estimate at time k
# into predicted sensor measurements at time k.
# In this case, H will be the identity matrix since the
# estimated state maps directly to state measurements data
# H has the same number of rows as sensor measurements
# and same number of columns as states.
H_k = np.array([[1.0, 0, 0, 0, 0],
[0, 1.0, 0, 0, 0],
[0, 0, 1.0, 0, 0],
[0, 0, 0, 1.0, 0],
[0, 0, 0, 0, 1.0]])
# Sensor measurement noise covariance matrix R_k
# Has the same number of rows and columns as sensor measurements.
# If we are sure about the measurements, R will be near zero.
R_k = np.array([[1.0, 0, 0, 0, 0],
[0, 1.0, 0, 0, 0],
[0, 0, 1.0, 0, 0],
[0, 0, 0, 1.0, 0],
[0, 0, 0, 0, 1.0]])
# Sensor noise. This is a vector with the
# number of elements equal to the number of sensor measurements.
sensor_noise_w_k = np.array([0.01, 0.01, 0.01, 0.01, 0.01])
def ekf(z_k_observation_vector, state_estimate_k_minus_1,
control_vector_k_minus_1, P_k_minus_1, dk):
######################### Predict #############################
# Predict the state estimate at time k based on the state
# estimate at time k-1 and the control input applied at time k-1.
state_estimate_k = A_k_minus_1 @ (
state_estimate_k_minus_1) + (
control_vector_k_minus_1) + (
process_noise_v_k_minus_1)
print(f'State Estimate Before EKF={state_estimate_k}')
# Predict the state covariance estimate based on the previous
# covariance and some noise
P_k = A_k_minus_1 @ P_k_minus_1 @ A_k_minus_1.T + (
Q_k)
################### Update (Correct) ##########################
# Calculate the difference between the actual sensor measurements
# at time k minus what the measurement model predicted
# the sensor measurements would be for the current timestep k.
measurement_residual_y_k = z_k_observation_vector - (
(H_k @ state_estimate_k) + (sensor_noise_w_k))
print(f'Observation={z_k_observation_vector}')
# Calculate the measurement residual covariance
S_k = H_k @ P_k @ H_k.T + R_k
# Calculate the near-optimal Kalman gain
# We use pseudoinverse since some of the matrices might be
# non-square or singular.
K_k = P_k @ H_k.T @ np.linalg.pinv(S_k)
# Calculate an updated state estimate for time k
state_estimate_k = state_estimate_k + (K_k @ measurement_residual_y_k)
# Update the state covariance estimate for time k
P_k = P_k - (K_k @ H_k @ P_k)
# Print the best (near-optimal) estimate of the current state of the robot
print(f'State Estimate After EKF={state_estimate_k}')
# Return the updated state and covariance estimates
return state_estimate_k, P_k
def main():
# We start at time k=1
k = 1
# Time interval in seconds
dk = 1
# Create a list of sensor observations at successive timesteps
# Each list within z_k is an observation vector.
z_k = np.array([[41.3920877, 2.165009483, 11.32350106, 190.255528, 1.05357508],
[41.39198647, 2.16505734, 12.49313626, 148.3052463, 1.169635197],
[41.3919087, 2.165148001, 11.48702769, 139.2730885, -1.006108567],
[41.39181576, 2.165256339, 13.72677596, 139.2730885, 2.239748268],
[41.39173316, 2.165357368, 12.46676741, 137.5553644, -1.260008547],
[41.39166792, 2.165438147, 9.901313858, 137.5553644, -2.565453555],
[41.39162805, 2.165487467, 6.047907658, 137.5827278, -3.8534062],
[41.39161926, 2.165498327, 1.332407586, 137.5954214, -4.715500072],
[41.39161926, 2.165498327, 0, 137.5954214, -1.332407586],
[41.39161926, 2.165498327, 0, 137.5954214, 0],
[41.391619, 2.165498, -1.930117, 137.595421, 1.023591],
[41.391619, 2.165498, -1.710974, 137.595421, 2.246764],
[41.391619, 2.165498, - 0.375267, 137.595421, 3.334014],
[41.391619, 2.165498, 2.287329, 137.595421, 4.192694],
[41.391619, 2.165498, 6.132224, 137.595421, 4.717386],
[41.391619, 2.165498, 7.416489, 137.595421, 4.833027],
[41.391619, 2.165498, 7.555815, 137.595421, 4.833027],
[41.391619, 2.165498, 9.685403, 137.595421, 4.833027],
[41.391619, 2.165498, 9.696774, 137.595421, 4.833027],
[41.391619, 2.165498, 8.403750, 137.595421, 4.833027]])
# The estimated state vector at time k-1 in the global reference frame.
# [x_k_minus_1, y_k_minus_1, yaw_k_minus_1]
# [meters, meters, radians]
state_estimate_k_minus_1 = np.array([0.0, 0.0, 0.0, 0.0, 0.0])
# The control input vector at time k-1 in the global reference frame.
# [v, yaw_rate]
# [meters/second, radians/second]
# In the literature, this is commonly u.
# Because there is no angular velocity and the robot begins at the
# origin with a 0 radians yaw angle, this robot is traveling along
# the positive x-axis in the global reference frame.
control_vector_k_minus_1 = np.array([41, 2, 0.0, 0.0, 0.0])
# State covariance matrix P_k_minus_1
# This matrix has the same number of rows (and columns) as the
# number of states. P is sometimes referred
# to as Sigma in the literature. It represents an estimate of
# the accuracy of the state estimate at time k made using the
# state transition matrix. We start off with guessed values.
P_k_minus_1 = np.array([[1.0, 0, 0, 0, 0],
[0, 1.0, 0, 0, 0],
[0, 0, 1.0, 0, 0],
[0, 0, 0, 1.0, 0],
[0, 0, 0, 0, 1.0]])
# Start at k=1 and go through each of the 20 sensor observations,
# one at a time.
# We stop right after timestep k=20 (i.e. the last sensor observation)
for k, obs_vector_z_k in enumerate(z_k, start=1):
# Print the current timestep
print(f'Timestep k={k}')
# Run the Extended Kalman Filter and store the
# near-optimal state and covariance estimates
optimal_state_estimate_k, covariance_estimate_k = ekf(
obs_vector_z_k, # Most recent sensor measurement
state_estimate_k_minus_1, # Our most recent estimate of the state
control_vector_k_minus_1, # Our most recent control input
P_k_minus_1, # Our most recent state covariance matrix
dk) # Time interval
# Get ready for the next timestep by updating the variable values
state_estimate_k_minus_1 = optimal_state_estimate_k
P_k_minus_1 = covariance_estimate_k
# Print a blank line
print()
# Program starts running here with the main method
main()
The output is:
Timestep k=1
State Estimate Before EKF=[41.01 2.01 0.01 0.01 0.01]
[ 41.392088 2.165009 11.323501 190.255528 1.053575]
Observation=[ 41.392088 2.165009 11.323501 190.255528 1.053575]
State Estimate After EKF=[ 41.258058 2.106673 7.545667 126.833685 0.69905 ]
Timestep k=2
State Estimate Before EKF=[ 82.268058 4.116673 7.555667 126.843685 0.70905 ]
[ 41.391986 2.165057 12.493136 148.305246 1.169635]
Observation=[ 41.391986 2.165057 12.493136 148.305246 1.169635]
State Estimate After EKF=[ 56.714263 2.890663 10.635335 140.250911 0.990666]
Timestep k=3
State Estimate Before EKF=[ 97.724263 4.900663 10.645335 140.260911 1.000666]
[ 41.391909 2.165148 11.487028 139.273089 -1.006109]
Observation=[ 41.391909 2.165148 11.487028 139.273089 -1.006109]
State Estimate After EKF=[ 62.845663 3.201059 11.160193 139.643211 -0.247814]
Timestep k=4
State Estimate Before EKF=[103.855663 5.211059 11.170193 139.653211 -0.237814]
[ 41.391816 2.165256 13.726776 139.273089 2.239748]
Observation=[ 41.391816 2.165256 13.726776 139.273089 2.239748]
State Estimate After EKF=[ 65.235466 3.322017 12.744444 139.412044 1.287588]
Timestep k=5
State Estimate Before EKF=[106.245466 5.332017 12.754444 139.422044 1.297588]
[ 41.391733 2.165357 12.466767 137.555364 -1.260009]
Observation=[ 41.391733 2.165357 12.466767 137.555364 -1.260009]
State Estimate After EKF=[ 66.156076 3.368665 12.570463 138.262152 -0.289329]
Timestep k=6
State Estimate Before EKF=[107.166076 5.378665 12.580463 138.272152 -0.279329]
[ 41.391668 2.165438 9.901314 137.555364 -2.565454]
Observation=[ 41.391668 2.165438 9.901314 137.555364 -2.565454]
State Estimate After EKF=[ 66.508869 3.386591 10.918469 137.82297 -1.698419]
Timestep k=7
State Estimate Before EKF=[107.518869 5.396591 10.928469 137.83297 -1.688419]
[ 41.391628 2.165487 6.047908 137.582728 -3.853406]
Observation=[ 41.391628 2.165487 6.047908 137.582728 -3.853406]
State Estimate After EKF=[ 66.643776 3.393477 7.905934 137.672131 -3.032636]
Timestep k=8
State Estimate Before EKF=[107.653776 5.403477 7.915934 137.682131 -3.022636]
[ 41.391619 2.165498 1.332408 137.595421 -4.7155 ]
Observation=[ 41.391619 2.165498 1.332408 137.595421 -4.7155 ]
State Estimate After EKF=[ 66.695326 3.396116 3.84091 137.622361 -4.075064]
Timestep k=9
State Estimate Before EKF=[107.705326 5.406116 3.85091 137.632361 -4.065064]
[ 41.391619 2.165498 0. 137.595421 -1.332408]
Observation=[ 41.391619 2.165498 0. 137.595421 -1.332408]
State Estimate After EKF=[ 66.71502 3.397124 1.464736 137.603351 -2.38237 ]
Timestep k=10
State Estimate Before EKF=[107.72502 5.407124 1.474736 137.613351 -2.37237 ]
[ 41.391619 2.165498 0. 137.595421 0. ]
Observation=[ 41.391619 2.165498 0. 137.595421 0. ]
State Estimate After EKF=[ 66.722543 3.397509 0.557119 137.59609 -0.912345]
Timestep k=11
State Estimate Before EKF=[107.732543 5.407509 0.567119 137.60609 -0.902345]
[ 41.391619 2.165498 -1.930117 137.595421 1.023591]
Observation=[ 41.391619 2.165498 -1.930117 137.595421 1.023591]
State Estimate After EKF=[ 66.725417 3.397656 -0.982438 137.593316 0.281769]
Timestep k=12
State Estimate Before EKF=[107.735417 5.407656 -0.972438 137.603316 0.291769]
[ 41.391619 2.165498 -1.710974 137.595421 2.246764]
Observation=[ 41.391619 2.165498 -1.710974 137.595421 2.246764]
State Estimate After EKF=[ 66.726515 3.397712 -1.435059 137.592256 1.493842]
Timestep k=13
State Estimate Before EKF=[107.736515 5.407712 -1.425059 137.602256 1.503842]
[ 41.391619 2.165498 -0.375267 137.595421 3.334014]
Observation=[ 41.391619 2.165498 -0.375267 137.595421 3.334014]
State Estimate After EKF=[ 66.726934 3.397733 -0.782432 137.591851 2.62877 ]
Timestep k=14
State Estimate Before EKF=[107.736934 5.407733 -0.772432 137.601851 2.63877 ]
[ 41.391619 2.165498 2.287329 137.595421 4.192694]
Observation=[ 41.391619 2.165498 2.287329 137.595421 4.192694]
State Estimate After EKF=[ 66.727094 3.397741 1.112424 137.591697 3.592968]
Timestep k=15
State Estimate Before EKF=[107.737094 5.407741 1.122424 137.601697 3.602968]
[ 41.391619 2.165498 6.132224 137.595421 4.717386]
Observation=[ 41.391619 2.165498 6.132224 137.595421 4.717386]
State Estimate After EKF=[ 66.727155 3.397744 4.21247 137.591638 4.285536]
Timestep k=16
State Estimate Before EKF=[107.737155 5.407744 4.22247 137.601638 4.295536]
[ 41.391619 2.165498 7.416489 137.595421 4.833027]
Observation=[ 41.391619 2.165498 7.416489 137.595421 4.833027]
State Estimate After EKF=[ 66.727178 3.397746 6.190302 137.591615 4.621543]
Timestep k=17
State Estimate Before EKF=[107.737178 5.407746 6.200302 137.601615 4.631543]
[ 41.391619 2.165498 7.555815 137.595421 4.833027]
Observation=[ 41.391619 2.165498 7.555815 137.595421 4.833027]
State Estimate After EKF=[ 66.727187 3.397746 7.031875 137.591607 4.749887]
Timestep k=18
State Estimate Before EKF=[107.737187 5.407746 7.041875 137.601607 4.759887]
[ 41.391619 2.165498 9.685403 137.595421 4.833027]
Observation=[ 41.391619 2.165498 9.685403 137.595421 4.833027]
State Estimate After EKF=[ 66.727191 3.397746 8.669485 137.591603 4.79891 ]
Timestep k=19
State Estimate Before EKF=[107.737191 5.407746 8.679485 137.601603 4.80891 ]
[ 41.391619 2.165498 9.696774 137.595421 4.833027]
Observation=[ 41.391619 2.165498 9.696774 137.595421 4.833027]
State Estimate After EKF=[ 66.727192 3.397746 9.302024 137.591602 4.817635]
Timestep k=20
State Estimate Before EKF=[107.737192 5.407746 9.312024 137.601602 4.827635]
[ 41.391619 2.165498 8.40375 137.595421 4.833027]
Observation=[ 41.391619 2.165498 8.40375 137.595421 4.833027]
State Estimate After EKF=[ 66.727193 3.397746 8.744499 137.591602 4.824787]
Actually, your Kalman is linear, not extended Kalman Filter.
Your problem is in the function ekf when you define the variable state_estimate_k.
The formula should be x(estimate) = F*x + B*u. Just change your code to this :
state_estimate_k = A_k_minus_1 @ (
state_estimate_k_minus_1) + (
control_vector_k_minus_1) @ (
process_noise_v_k_minus_1)