pos_b = (-100, -20)
f1 = KalmanFilter(dim_x=4, dim_z=2)
f1.F = np.mat ([[0, 1, 0, 0],
[0, 0, 0, 0],
[0, 0, 0, 1],
[0, 0, 0, 0]])
f1.B = 0.
f1.R *= 1.
f1.Q *= .1
f1.x = np.mat([1,0,1,0]).T
f1.P = np.eye(4) * 5.
# initialize storage and other variables for the run
count = 30
xs, ys = [],[]
pxs, pys = [],[]
# create the simulated sensor
d = DMESensor (pos_a, pos_b, noise_factor=1.)
# pos will contain our nominal position since the filter does not
# maintain position.
pos = [0,0]
for i in range(count):
# move (1,1) each step, so just use i
def main():
print("Start systemmodel main")
timestep = 0.01
car1 = Vechile(26, timestep, x=0, y=0, gamma=math.pi / 4)
car1Err = Vechile(26, timestep, x=0, y=0, gamma=math.pi / 4)
alpha_a, alpha_a_err, accel_a, accel_a_err = testSecvenceGenerate(timestep)
# The vechile position and oriantation without error
X_car1 = []
Y_car1 = []
Gamma_car1 = []
W_car1 = [0]
# The vechile position and oriantation with error
X_car1Err = []
Y_car1Err = []
Gamma_car1Err = []
W_car1Err = [0]
# The vechile position and oriantation with error
X_car1ErrF = []
Y_car1ErrF = []
Gamma_car1ErrF = []
W_car1ErrF = [0]
plt.figure()
ax = plt.subplot(111, aspect='equal')
X_car1ErrF.append(0)
Y_car1ErrF.append(0)
Gamma_car1ErrF.append(math.pi / 4)
X_car1Err.append(0)
Y_car1Err.append(0)
Gamma_car1Err.append(math.pi / 4)
X_car1.append(0)
Y_car1.append(0)
Gamma_car1.append(math.pi / 4)
# State covariance
P = getStateCovariance()
Q = getProcessNoise(0.0, timestep, math.pi / 180 * 100, 26, 1.0)
R = getMeasurmentNoise(velf=0.00001, groErr=math.pi / 360 / 5)
# e1 = plotHelp.plotEllipse(0, 0, P[1:3, 1:3])
# ax.add_artist(e1)
l_kalmanFilter = KalmanFilter(car1Err.state, car1Err.f, car1Err.h,
car1Err.F_x, car1Err.F_u, car1Err.H, P, Q, R,
Vechile.State, Vechile.Output)
Velf = [0]
VelfErr = [0]
VelfErrF = [0]
for accel, alpha in zip(accel_a, alpha_a):
l_input = Vechile.Input(accel, alpha)
newState = car1.f(car1.state, l_input)
l_measurment = car1.h(car1.state)
car1.state = newState
# Position without error
X_car1.append(newState.x)
Y_car1.append(newState.y)
Gamma_car1.append(newState.gamma)
Velf.append(newState.vf)
W_car1.append(newState.w)
Vf_err, W_err = testhelp.generateMesError(Velf, 0.0, 0.0, W_car1, 0.00,
math.pi / 360 / 5)
for accel_err, alpha_err, vf_mes, w_mes in zip(accel_a_err, alpha_a_err,
Vf_err, W_err):
l_measurment = Vechile.Output(vf_mes, w_mes)
l_input = Vechile.Input(accel_err, alpha_err)
newStateErr = car1Err.f(car1Err.state, l_input)
car1Err.state = newStateErr
X_car1Err.append(newStateErr.x)
Y_car1Err.append(newStateErr.y)
Gamma_car1Err.append(newStateErr.gamma)
VelfErr.append(newStateErr.vf)
W_car1Err.append(newStateErr.w)
newStateErrF, l_P = l_kalmanFilter.predict(l_kalmanFilter.X, l_input,
l_kalmanFilter.P)
# print('Angle', P[3, 3])
newStateErrF, l_P = l_kalmanFilter.update(newStateErrF, l_measurment,
l_P)
l_kalmanFilter.X = newStateErrF
l_kalmanFilter.P = l_P
X_car1ErrF.append(newStateErrF.x)
Y_car1ErrF.append(newStateErrF.y)
Gamma_car1ErrF.append(newStateErrF.gamma)
VelfErrF.append(newStateErrF.vf)
W_car1ErrF.append(newStateErrF.w)
plt.plot(X_car1, Y_car1, '--ro')
plt.plot(X_car1Err, Y_car1Err, '--go')
plt.plot(X_car1ErrF, Y_car1ErrF, '--bo')
plt.legend(['Real', 'Error', 'Filtered'])
plt.figure()
plt.title('Angle speed')
plt.plot(W_car1)
plt.plot(W_err)
plt.plot(W_car1Err)
plt.plot(W_car1ErrF)
plt.legend(['Real', 'Added Error', 'Error based pred.', 'Filtered'])
plt.figure()
plt.title('Oriantation')
plt.plot(Gamma_car1)
plt.plot(Gamma_car1Err)
plt.plot(Gamma_car1ErrF)
plt.legend(['Real', 'Error', 'Filtered'])
plt.figure()
plt.title('Velocity')
plt.plot(Velf)
plt.plot(Vf_err)
plt.plot(VelfErr)
plt.plot(VelfErrF)
plt.legend(['Real', 'Added Error', 'Error based pred.', 'Filtered'])
posErr = []
posErrF = []
for x_p, y_p, x_pf, y_pf, x_pR, y_pR in zip(X_car1, Y_car1, X_car1ErrF,
Y_car1ErrF, X_car1Err,
Y_car1Err):
p = complex(x_p, y_p)
pf = complex(x_pf, y_pf)
pr = complex(x_pR, y_pR)
errorF = abs(p - pf)
error = abs(p - pr)
posErr.append(error)
posErrF.append(errorF)
# #
plt.figure()
plt.plot(posErr)
plt.plot(posErrF)
plt.legend(['Error', 'ErrorF'])
plt.show()
print("End systemmodel main")
pos_b = (-100, -20)
f1 = KalmanFilter(dim_x=4, dim_z=2)
f1.F = np.mat ([[0, 1, 0, 0],
[0, 0, 0, 0],
[0, 0, 0, 1],
[0, 0, 0, 0]])
f1.B = 0.
f1.R *= 1.
f1.Q *= .1
f1.x = np.mat([1,0,1,0]).T
f1.P = np.eye(4) * 5.
# initialize storage and other variables for the run
count = 30
xs, ys = [],[]
pxs, pys = [],[]
# create the simulated sensor
d = DMESensor (pos_a, pos_b, noise_factor=1.)
# pos will contain our nominal position since the filter does not
# maintain position.
pos = [0,0]
for i in range(count):
# move (1,1) each step, so just use i
