est_k] # last state estimate vector from robot frame
P = est_rbt_P[:, :, est_k] # last covariance matrix
Q = est_rbt_Q # process noise covariance matrix
R = est_rbt_R # measurement noise covariance
H = est_rbt_H # observation matrix
z = est_rbt_m[:, est_k] # measurement vector
u = matrix(
[[0], [0], [0]]
) # control input vector (don't give kalman filter knowledge about thruster inputs)
A = est_rbt_A
B = est_rbt_B
state = KalmanFilter(x, P, z, u, A, B, Q, R, H)
x, P = state.predict(x, P, u)
x, K, P = state.update(x, P, z)
# print('x', x)
# # print('P', P)
# # print('Q', Q)
# # print('R', R)
# # print('H', H)
# # print('z', z)
# # print('u', u)
# # print('K', K)
est_rbt_x[:, est_k + 1] = x
est_rbt_L[:, :, est_k + 1] = K
est_rbt_P[:, :, est_k + 1] = P
# Compute for map position
no_of_state_measurements=no_of_state_measurements)
step = 0 # step counter, used for simulation of abscent measurements only
# Loop that runs throughout the time array defined previously
for t in time:
#Simulate real speed
real_speed_x = initial_speed_x + np.random.randn() * np.sqrt(
acceleration_variance)
real_speed_y = initial_speed_y + np.random.randn() * np.sqrt(
acceleration_variance)
# Simulate real position
real_pos_x = real_pos_x + dt * real_speed_x
real_pos_y = real_pos_y + dt * real_speed_y
# Simulate measurements with noise
meas_value_x = real_pos_x + np.random.randn() * np.sqrt(meas_variance)
meas_value_y = real_pos_y + np.random.randn() * np.sqrt(meas_variance)
# Simulation of abscent measurements
if step > 70 and step < 110:
meas_value_x, meas_value_y = 0, 0
# Assign obtained measurements to measurement array
z = np.array([meas_value_x, meas_value_y])
kf.predict(dt=dt)
kf.update(state_measurement=z)
step += 1 # increment step
