Skip to content

Instantly share code, notes, and snippets.

@lava
Created February 4, 2013 11:44
Show Gist options
  • Save lava/4706289 to your computer and use it in GitHub Desktop.
Save lava/4706289 to your computer and use it in GitHub Desktop.
Python and CPP kalman filter update
def kalman_update(state_mean, state_cov, measurement, measurement_matrix, meas_cov):
# meas_cov should be a matrix with the right dimensions
statedim = state_mean.size
x = reshape( atleast_2d(state_mean), (statedim,1) )
C = atleast_2d( state_cov )
H = atleast_2d(measurement_matrix)
y = atleast_2d(np.array(measurement))
innovation = y - dot(H,x)
S = linalg.dot(linalg.dot(H,C),H.transpose()) + meas_cov
K = linalg.dot(linalg.dot(C,H.transpose()),linalg.inv(S))
xEst = x + dot(K, innovation)
CEst = C - dot(dot(K, H), C) # Wikipedia/LMA skript formula
return [xEst, CEst]
void kalman_update(const StateMean& x, const StateCovariance& C, const Measurement& y, const MeasurementMatrix& H, const MeasurementCovariance& Q, StateMean& xe, StateCovariance& Ce) {
auto innovation = y - H*x;
auto S = H*C*H.transpose() + Q;
auto K = C*H.transpose()*S.inverse();
xe = x + K*innovation;
Ce = C - K*H*C;
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment