Differences

This shows you the differences between two versions of the page.

--- code:kalman_filter [2008/07/19 23:33] – external edit 127.0.0.1
+++ code:kalman_filter [2008/10/20 13:38] (current) – laurenceb
@@ Line 18: / Line 18: @@
  The simulated gyro output in red, and filter turn rate output in green.
@@ Line 26: / Line 30: @@
-kalman_state predict_and_update(kalman_state * Prev, kalman_model * model, vector * u, vector * y)
+void predict_and_update(kalman_state * Prev, kalman_model * model, vector * u, vector * y)
 {
-	kalman_state New;
+	predict_state(model,Prev,u);
-	matrix K;
+	predict_P(model,Prev);
-	New.state=predict_state(model,Prev,u);
+	matrix K=optimal_gain(model,Prev);
-	New.P=predict_P(model,Prev);
+	state_update(Prev,model,&K,y);
-	K=optimal_gain(model,&New);
+	P_update(&K,model,Prev);
-	New.state=state_update(&New,model,&K,y);
-	New.P=P_update(&K,model,&New);
-	return New;
 }
-vector predict_state(kalman_model * model, kalman_state * state, vector * u)		//x is system state, u control input(s), B input model
+void predict_state(kalman_model * model, kalman_state * state, vector * u)	//x is system state, u control input(s), B input model
 {
 	vector a;
@@ Line 44: / Line 45: @@
 	a=matrix_vector(&model->F,&state->state);
 	b=matrix_vector(&model->B,u);
-	return vector_add(&a,&b);							//estimated state
+	state->state = vector_add(&a,&b);					//estimated state
 }
-matrix predict_P(kalman_model * model, kalman_state * state)		//F is propagation model, Q model noise
+void predict_P(kalman_model * model, kalman_state * state)		//F is propagation model, Q model noise
 {
 	matrix S=transpose(&model->F);
 	S=multiply(&state->P,&S);
 	S=multiply(&model->F,&S);
-	return add(&S,&model->Q);					//estimated estimate covariance
+	state->P = add(&S,&model->Q);					//estimated estimate covariance
 }
@@ Line 60: / Line 61: @@
 	matrix S=multiply(&state->P,&T);
 	S=multiply(&model->H,&S);
-	S=add(&model->R,&S);				//S is now the innovation covariance
+	S=add(&model->R,&S);						//S is now the innovation covariance
 	S=inverse(&S);
 	S=multiply(&T,&S);
-	return multiply(&state->P,&S);			//the optimal gain
+	return multiply(&state->P,&S);					//the optimal gain
 }
-vector state_update(kalman_state * state, kalman_model * model, matrix * K, vector * y)	//y is the measurements
+void state_update(kalman_state * state, kalman_model * model, matrix * K, vector * y)	//y is the measurements
 {
 	vector a=matrix_vector(&model->H,&state->state);
 	a=vector_subtract(y,&a);						//a is now the difference or residual
+	if(a.t>PI)								//need to make sure we stay in the correct range
+	{
+		a.t-=2*PI;
+	}
+	if(a.t<PI)
+	{
+		a.t+=2*PI;
+	}
 	a=matrix_vector(K,&a);
-	return vector_add(&state->state,&a);
+	state->state = vector_add(&state->state,&a);
 }
-matrix P_update(matrix *K, kalman_model * model, kalman_state * state)		//K is optimal gain, H measurement model
+void P_update(matrix *K, kalman_model * model, kalman_state * state)		//K is optimal gain, H measurement model
 {
 	matrix S=multiply(K,&model->H);
-	S.tl=1-S.tl;					//subtraction from identity
+	S.tl=1.0-S.tl;					//subtraction from identity
 	S.tr=-S.tr;
 	S.bl=-S.bl;
-	S.br=1-S.br;
+	S.br=1.0-S.br;
-	return multiply(&S,&state->P);
+	state->P = multiply(&S,&state->P);
 }
 </code>
 ===== Kalman.h =====
@@ Line 103: / Line 113: @@
 } kalman_model;
-kalman_state predict_and_update(kalman_state * Prev, kalman_model * model, vector * u, vector * y);
+void predict_and_update(kalman_state * Prev, kalman_model * model, vector * u, vector * y);
-vector predict_state(kalman_model * model, kalman_state * state, vector * u);
+void predict_state(kalman_model * model, kalman_state * state, vector * u);
-matrix predict_P(kalman_model * model, kalman_state * state);
+void predict_P(kalman_model * model, kalman_state * state);
 matrix optimal_gain(kalman_model * model, kalman_state * state);
-vector state_update(kalman_state * state, matrix * K, vector * y);
+void state_update(kalman_state * state, matrix * K, vector * y);
-matrix P_update(matrix *K, kalman_model * model, kalman_state * state);
+void P_update(matrix *K, kalman_model * model, kalman_state * state);
 </code>