Function rc_kalman_update_lin

• Defined in File kalman.h

Function Documentation

int rc_kalman_update_lin(rc_kalman_t *kf, rc_vector_t u, rc_vector_t y)

Kalman Filter state prediction step based on physical model.

Uses the state estimate and control input from the previous timestep to produce an estimate of the state at the current timestep. This step pdates P and the estimated state x. Assume that you have calculated f(x[k|k],u[k]) and F(x[k|k],u[k]) before calling this function.

• Kalman linear state prediction

  • x_pre[k|k-1] = F*x[k-1|k-1] + G*u[k-1]

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

• Kalman measurement Update:

  • h[k] = H * x_pre[k]

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

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

  • x_est[k|k] = x[k|k-1] + L*(y[k]-h[k])

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

Parameters:

• kf – pointer to struct to be updated

• u – control input

• y – [in] sensor measurement

Returns:

0 on success, -1 on failure