void b2_hff_update_gps(struct FloatVect2 *pos_ned, struct FloatVect2 *speed_ned)
{
b2_hff_lost_counter = 0;
#if USE_GPS_ACC4R
Rgps_pos = (float) gps.pacc / 100.;
if (Rgps_pos < HFF_R_POS_MIN) {
Rgps_pos = HFF_R_POS_MIN;
}
Rgps_vel = (float) gps.sacc / 100.;
if (Rgps_vel < HFF_R_SPEED_MIN) {
Rgps_vel = HFF_R_SPEED_MIN;
}
#endif
#ifdef GPS_LAG
if (GPS_LAG_N == 0) {
#endif
/* update filter state with measurement */
b2_hff_update_x(&b2_hff_state, pos_ned->x, Rgps_pos);
b2_hff_update_y(&b2_hff_state, pos_ned->y, Rgps_pos);
#if HFF_UPDATE_SPEED
b2_hff_update_xdot(&b2_hff_state, speed_ned->x, Rgps_vel);
b2_hff_update_ydot(&b2_hff_state, speed_ned->y, Rgps_vel);
#endif
#ifdef GPS_LAG
} else if (b2_hff_rb_n > 0) {
/* roll back if state was saved approx when GPS was valid */
lag_counter_err = b2_hff_rb_last->lag_counter - GPS_LAG_N;
PRINT_DBG(2, ("update. rb_n: %d lag_counter: %d lag_cnt_err: %d\n", b2_hff_rb_n, b2_hff_rb_last->lag_counter,
lag_counter_err));
if (abs(lag_counter_err) <= GPS_LAG_TOL_N) {
b2_hff_rb_last->rollback = true;
b2_hff_update_x(b2_hff_rb_last, pos_ned->x, Rgps_pos);
b2_hff_update_y(b2_hff_rb_last, pos_ned->y, Rgps_pos);
#if HFF_UPDATE_SPEED
b2_hff_update_xdot(b2_hff_rb_last, speed_ned->x, Rgps_vel);
b2_hff_update_ydot(b2_hff_rb_last, speed_ned->y, Rgps_vel);
#endif
past_save_counter = GPS_DT_N - 1; // + lag_counter_err;
PRINT_DBG(2, ("gps updated. past_save_counter: %d\n", past_save_counter));
b2_hff_propagate_past(b2_hff_rb_last);
} else if (lag_counter_err >= GPS_DT_N - (GPS_LAG_TOL_N + 1)) {
/* apparently missed a GPS update, try next saved state */
PRINT_DBG(2, ("try next saved state\n"));
b2_hff_rb_drop_last();
b2_hff_update_gps(pos_ned, speed_ned);
}
} else if (save_counter < 0) {
/* ringbuffer empty -> save output filter state at next GPS validity point in time */
save_counter = GPS_DT_N - 1 - (GPS_LAG_N % GPS_DT_N);
PRINT_DBG(2, ("rb empty, save counter set: %d\n", save_counter));
}
#endif /* GPS_LAG */
}
/*
*
* Update position
*
*
H = [1 0];
R = 0.1;
// state residual
y = pos_measurement - H * Xm;
// covariance residual
S = H*Pm*H' + R;
// kalman gain
K = Pm*H'*inv(S);
// update state
Xp = Xm + K*y;
// update covariance
Pp = Pm - K*H*Pm;
*/
void b2_hff_update_pos (struct FloatVect2 pos, struct FloatVect2 Rpos) {
b2_hff_update_x(&b2_hff_state, pos.x, Rpos.x);
b2_hff_update_y(&b2_hff_state, pos.y, Rpos.y);
}
void b2_hff_update_gps(void) {
b2_hff_lost_counter = 0;
#if USE_GPS_ACC4R
Rgps_pos = (float) gps.pacc / 100.;
if (Rgps_pos < HFF_R_POS_MIN)
Rgps_pos = HFF_R_POS_MIN;
Rgps_vel = (float) gps.sacc / 100.;
if (Rgps_vel < HFF_R_SPEED_MIN)
Rgps_vel = HFF_R_SPEED_MIN;
#endif
#ifdef GPS_LAG
if (GPS_LAG_N == 0) {
#endif
/* update filter state with measurement */
b2_hff_update_x(&b2_hff_state, ins_gps_pos_m_ned.x, Rgps_pos);
b2_hff_update_y(&b2_hff_state, ins_gps_pos_m_ned.y, Rgps_pos);
#ifdef HFF_UPDATE_SPEED
b2_hff_update_xdot(&b2_hff_state, ins_gps_speed_m_s_ned.x, Rgps_vel);
b2_hff_update_ydot(&b2_hff_state, ins_gps_speed_m_s_ned.y, Rgps_vel);
#endif
/* update ins state */
ins_ltp_accel.x = ACCEL_BFP_OF_REAL(b2_hff_state.xdotdot);
ins_ltp_accel.y = ACCEL_BFP_OF_REAL(b2_hff_state.ydotdot);
ins_ltp_speed.x = SPEED_BFP_OF_REAL(b2_hff_state.xdot);
ins_ltp_speed.y = SPEED_BFP_OF_REAL(b2_hff_state.ydot);
ins_ltp_pos.x = POS_BFP_OF_REAL(b2_hff_state.x);
ins_ltp_pos.y = POS_BFP_OF_REAL(b2_hff_state.y);
#ifdef GPS_LAG
} else if (b2_hff_rb_n > 0) {
/* roll back if state was saved approx when GPS was valid */
lag_counter_err = b2_hff_rb_last->lag_counter - GPS_LAG_N;
PRINT_DBG(2, ("update. rb_n: %d lag_counter: %d lag_cnt_err: %d\n", b2_hff_rb_n, b2_hff_rb_last->lag_counter, lag_counter_err));
if (abs(lag_counter_err) <= GPS_LAG_TOL_N) {
b2_hff_rb_last->rollback = TRUE;
b2_hff_update_x(b2_hff_rb_last, ins_gps_pos_m_ned.x, Rgps_pos);
b2_hff_update_y(b2_hff_rb_last, ins_gps_pos_m_ned.y, Rgps_pos);
#ifdef HFF_UPDATE_SPEED
b2_hff_update_xdot(b2_hff_rb_last, ins_gps_speed_m_s_ned.x, Rgps_vel);
b2_hff_update_ydot(b2_hff_rb_last, ins_gps_speed_m_s_ned.y, Rgps_vel);
#endif
past_save_counter = GPS_DT_N-1;// + lag_counter_err;
PRINT_DBG(2, ("gps updated. past_save_counter: %d\n", past_save_counter));
b2_hff_propagate_past(b2_hff_rb_last);
} else if (lag_counter_err >= GPS_DT_N - (GPS_LAG_TOL_N+1)) {
/* apparently missed a GPS update, try next saved state */
PRINT_DBG(2, ("try next saved state\n"));
b2_hff_rb_drop_last();
b2_hff_update_gps();
}
} else if (save_counter < 0) {
/* ringbuffer empty -> save output filter state at next GPS validity point in time */
save_counter = GPS_DT_N-1 - (GPS_LAG_N % GPS_DT_N);
PRINT_DBG(2, ("rb empty, save counter set: %d\n", save_counter));
}
#endif /* GPS_LAG */
}
