This is the latest (main) BeagleBoard documentation. If you are looking for stable releases, use the drop-down menu on the bottom-left and select the desired version.

Function rc_kalman_update_ekf

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