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