Differences

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

--- code:kalman_filter [2008/09/10 22:32] – 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);