Differences

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

--- code:kalman_filter [2008/09/10 22:31] – laurenceb
+++ 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 68: / Line 70: @@
 {
 	vector a=matrix_vector(&model->H,&state->state);
-	a=vector_subtract(y,&state->state);						//a is now the difference or residual
+	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);
 	state->state = vector_add(&state->state,&a);
@@ Line 83: / Line 93: @@
 }
 </code>
 ===== Kalman.h =====
@@ Line 102: / 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>