Change from jerk model to std noise model
@@ -531,17 +531,23 @@ void survive_kalman_tracker_process_noise(const struct SurviveKalmanTracker_Para
// http://wiki.dmdevelopment.ru/wiki/Download/Books/Digitalimageprocessing/%D0%9D%D0%BE%D0%B2%D0%B0%D1%8F%20%D0%BF%D0%BE%D0%B4%D0%B1%D0%BE%D1%80%D0%BA%D0%B0%20%D0%BA%D0%BD%D0%B8%D0%B3%20%D0%BF%D0%BE%20%D1%86%D0%B8%D1%84%D1%80%D0%BE%D0%B2%D0%BE%D0%B9%20%D0%BE%D0%B1%D1%80%D0%B0%D0%B1%D0%BE%D1%82%D0%BA%D0%B5%20%D1%81%D0%B8%D0%B3%D0%BD%D0%B0%D0%BB%D0%BE%D0%B2/Estimation%20with%20Applications%20to%20Tracking%20and%20Navigation/booktext@id89013302placeboie.pdf
// We mix three order models here based on tuning variables.
- FLT Q_acc[] = {t5 / 20., t4 / 8., t3 / 6., t3 / 3., t2 / 2., t};
+ FLT Q_acc[] = {
+ t4 / 4.,
+ t3 / 2., t2,
+ t2 / 2., t, 1
+ };
+ FLT Q_vel[] = {
+ t3 / 3.,
+ t2 / 2., t,
+ };
- FLT Q_vel[] = {t3 / 3., t2 / 2., t};
+ FLT p_p = params->process_weight_acc * Q_acc[0] + params->process_weight_vel * Q_vel[0] + params->process_weight_pos * t;
+ FLT p_v = params->process_weight_acc * Q_acc[1] + params->process_weight_vel * Q_vel[1];
+ FLT p_a = params->process_weight_acc * Q_acc[3];
- FLT q_p = params->process_weight_acc;
- FLT p_p = q_p * Q_acc[0] + params->process_weight_vel * Q_vel[0] + params->process_weight_pos * t;
- FLT p_v = q_p * Q_acc[1] + params->process_weight_vel * Q_vel[1];
- FLT p_a = q_p * Q_acc[2];
- FLT v_v = q_p * Q_acc[3] + params->process_weight_vel * Q_vel[2];
- FLT v_a = q_p * Q_acc[4];
- FLT a_a = q_p * Q_acc[5];
+ FLT v_v = params->process_weight_acc * Q_acc[2] + params->process_weight_vel * Q_vel[2];
+ FLT v_a = params->process_weight_acc * Q_acc[4];
+ FLT a_a = params->process_weight_acc * Q_acc[5];
/* ================== Rotational ==============================
