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")