示例#1
0
        plt.pause(plotpause)

# %% setup and track

# Model parameters EKF
sigma_a = 3  # From EKF exc 3
sigma_z = 10  # From EKF exc 3

# PDA relevant
PD = 0.9
clutter_intensity = 10e-6  # TODO # Basically estimated from poisson clutter model, makes no sense to have fixed one
gate_size = 3

CV = dynamicmodels.WhitenoiseAccelleration(sigma_a)
measurement_model = measurementmodels.CartesianPosition(sigma_z)
ekf_filter = ekf.EKF(CV, measurement_model)

tracker = pda.PDA(ekf_filter, clutter_intensity, PD, gate_size)

# initialize
z_0 = Z[0]
x_bar_init = np.array([*z_0[0], 0, 0])

P_bar_init = np.zeros((4, 4))
P_bar_init[[0, 1], [0, 1]] = 2 * sigma_z**2
P_bar_init[[2, 3], [2, 3]] = 5**2

init_state = tracker.init_filter_state({"mean": x_bar_init, "cov": P_bar_init})

# allocate
NEES = np.zeros(K)
示例#2
0
cov_init = np.diag([1000, 1000, 30, 30, 0.1])**2  # THIS WILL NOT BE GOOD
mode_probabilities_init = np.array([p10, (1 - p10)])
mode_states_init = GaussParams(mean_init, cov_init)
init_imm_state = MixtureParameters(mode_probabilities_init,
                                   [mode_states_init] * 2)

assert np.allclose(np.sum(mode_probabilities_init),
                   1), "initial mode probabilities must sum to 1"

# make model
measurement_model = measurementmodels.CartesianPosition(sigma_z, state_dim=5)
dynamic_models: List[dynamicmodels.DynamicModel] = []
dynamic_models.append(dynamicmodels.WhitenoiseAccelleration(sigma_a_CV, n=5))
dynamic_models.append(dynamicmodels.ConstantTurnrate(sigma_a_CT, sigma_omega))
ekf_filters = []
ekf_filters.append(ekf.EKF(dynamic_models[0], measurement_model))
ekf_filters.append(ekf.EKF(dynamic_models[1], measurement_model))
imm_filter = imm.IMM(ekf_filters, PI)

tracker = pda.PDA(imm_filter, clutter_intensity, PD, gate_size)

# init_imm_pda_state = tracker.init_filter_state(init__immstate)

NEES = np.zeros(K)
NEESpos = np.zeros(K)
NEESvel = np.zeros(K)

tracker_update = init_imm_state
tracker_update_list = []
tracker_predict_list = []
tracker_estimate_list = []
示例#3
0
# IMM Init
init_mean = np.array([0, 0, 0, 0, 0])

# IMM Init Covariance
init_cov = np.zeros((state_dim, state_dim))  # 5 x 5 if CV and CT modes
init_cov[[0, 1], [0, 1]] = 2 * sigma_z ** 2
init_cov[[2, 3], [2, 3]] = 3
init_cov[state_dim-1, state_dim-1] = 0.001 ** 2  # 5 th diag element


# Models and filter instantiation
measurement_model = measurementmodels.CartesianPosition(sigma_z, state_dim=state_dim)
CV = dynamicmodels.WhitenoiseAccelleration(sigma_a_CV, n=state_dim)
CT = dynamicmodels.ConstantTurnrate(sigma_a_CT, sigma_omega)
ekf_filters = [ekf.EKF(CV, measurement_model), ekf.EKF(CT, measurement_model)]

# Transition matrix test
assert np.allclose(PI.sum(axis=1), 1), "rows of PI must sum to 1"
assert PI.shape[0] == modes, 'Dimension transition matrix not same as modes'

# IMM filter instantiation
imm_filter = imm.IMM(ekf_filters, PI)

# IMM init mode probabilities test
assert np.allclose(np.sum(init_weights), 1), \
    "initial mode probabilities must sum to 1"

init_mode_states = [GaussParams(init_mean, init_cov)] * modes
init_immstate = MixtureParameters(init_weights, init_mode_states)  # Mixture of two mode states
tracker = pda.PDA(imm_filter, clutter_intensity, PD, gate_size)
示例#4
0
init_state_CV = GaussParams(init_mean[:4],
                            init_cov[:4, :4])  # get rid of turn rate
init_state_CT = GaussParams(init_mean, init_cov)  # same init otherwise
init_states = [init_state_CV, init_state_CT]

# create models
measurement_model_CV = measurementmodels.CartesianPosition(sigma_z)
measurement_model_CT = measurementmodels.CartesianPosition(sigma_z,
                                                           state_dim=5)
CV = dynamicmodels.WhitenoiseAccelleration(sigma_a_CV)
CT = dynamicmodels.ConstantTurnrate(sigma_a_CT, sigma_omega)

# create filters
filters = []
filters.append(ekf.EKF(CV, measurement_model_CV))
filters.append(ekf.EKF(CT, measurement_model_CT))

# allocate
pred = []
upd = []
NIS = np.empty((2, K))
NEES_pred = np.empty((2, K))
NEES_upd = np.empty((2, K))
err_pred = np.empty((2, 2, K))  # (filters, vel/pos, time)
err_upd = np.empty((2, 2, K))  # (filters, vel/pos, time)

# per filter
for i, (ekf_filter, init) in enumerate(zip(filters, init_states)):
    # setup per filter
    updated = init
示例#5
0
# IMM Init from initial position measurement / WHY NOT VELOCITY?
Z_0 = Z[0]
init_mean = np.array([Z_0[0, 0], Z_0[0, 1], 2, 0, 0])

# IMM Init Covariance
init_cov = np.diag([sigma_z, sigma_z, 3, 3,
                    0.0005])**2  # THIS WILL NOT BE GOOD

# Models and filter instantiation
measurement_model = measurementmodels.CartesianPosition(sigma_z,
                                                        state_dim=state_dim)
CV = dynamicmodels.WhitenoiseAccelleration(sigma_a_CV, n=state_dim)
CV_high = dynamicmodels.WhitenoiseAccelleration(sigma_a_CV_high, n=state_dim)
CT = dynamicmodels.ConstantTurnrate(sigma_a_CT, sigma_omega)
ekf_filters = [
    ekf.EKF(CV, measurement_model),
    ekf.EKF(CT, measurement_model),
    ekf.EKF(CV_high, measurement_model)
]

# Transition matrix test
assert np.allclose(PI.sum(axis=1), 1), "rows of PI must sum to 1"
assert PI.shape[0] == modes, 'Dimension transition matrix not same as modes'

# IMM filter instantiation
imm_filter = imm.IMM(ekf_filters, PI)

# IMM init mode probabilities test
assert np.allclose(np.sum(init_weights), 1), \
    "initial mode probabilities must sum to 1"