Function rc_kalman_update_ekf

• Defined in File kalman.h

Function Documentation

int rc_kalman_update_ekf(rc_kalman_t *kf, rc_matrix_t F, rc_matrix_t H, rc_vector_t x_pre, rc_vector_t y, rc_vector_t h)

Kalman Filter measurement update step.

Updates L, P, & x_est. Assumes that you have done the non-linear prediction step in your own function which should calculate the Jacobians F(x[k|k-1]) & H(x[k|k-1]), the predicted sensor value h(x[k|k-1]), and of course the predicted state x_pre[k|k-1]

-Kalman measurement Update:

• P[k|k-1] = F*P[k-1|k-1]*F^T + Q

• S = H*P*H^T + R

• L = P*(H^T)*(S^-1)

• x[k|k] = x[k|k-1] + L*y

• P[k|k] = (I - L*H)*P

Also updates the step counter in the rc_kalman_t struct

Parameters:

• kf – pointer to struct to be updated

• F – [in] Jacobian of state transition matrix linearized at x_pre

• H – [in] Jacobian of observation matrix linearized at x_pre

• x_pre – [in] predicted state

• y – [in] new sensor data

• h – [in] Ideal estimate of y, usually h=H*x_pre.

Returns:

0 on success, -1 on failure