Python and CPP kalman filter update

gistfile1.txt

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;
}