Matrix stuff

acm-uiuc · Mar 7, 2012 · ce48fff · ce48fff
1 parent fb4a09c
commit ce48fff
Show file tree

Hide file tree

Showing 2 changed files with 75 additions and 40 deletions.
diff --git a/main/main_kalman/kalman.pde b/main/main_kalman/kalman.pde
@@ -249,32 +249,32 @@ math.MatrixSubtract((float*)ProcessedSensorData, (float*)temp4, m, 1, (float*)te
 // innovation or residual covariance
 math.MatrixMult((float*)H, (float*)P_k_1, m, n, n, (float*)tempmn); 
 
-//Serial.println("ResCov");
+Serial.println("ResCov");
 math.MatrixMult((float*)H_t, (float*)tempnm, m, n, m, (float*)tempm);
 math.MatrixAdd((float*)tempm, (float*)R, m, m, (float*)tempm);
 
 // Optimal Kalman gain
 math.MatrixInvert((float*)tempm, m);
 math.MatrixMult((float*)P_k_1, (float*)tempnm, n, n, m, (float*)tempnm2);
 math.MatrixMult((float*)tempnm2, (float*)tempm2, n, m, m, (float*)tempnm);
-//Serial.println("KGain");
+Serial.println("KGain");
 // updated state estimate
-math.MatrixMult((float*)tempnm, (float*)temp2, m, m, 1, (float*)X_k);
-math.MatrixAdd((float*)X_k, (float*)X_k_1, m, 1, (float*)X_k);
-//Serial.println("State");
+math.MatrixMult((float*)tempnm, (float*)temp4, n, m, 1, (float*)X_k);
+math.MatrixAdd((float*)X_k, (float*)X_k_1, n, 1, (float*)X_k);
+Serial.println("State");
 //updated estimate covarience
-math.MatrixMult((float*)tempnm, (float*)H, n, m, n, (float*)temp1);
+math.MatrixMult((float*)tempnm, (float*)H, n, m, n, (float*)temp2);
 
 int i, j;
   for(i = 0; i < n; i++){
     for(j = 0; j < n; j++){
-      temp1[i][j] = -temp1[i][j];
+      temp2[i][j] = -temp2[i][j];
     }
   }
   for(i = 0; i < n; i++){
-    temp1[i][i] +=1;
+    temp2[i][i] +=1;
   }
 
-math.MatrixMult((float*)temp1, (float*)P_k_1, n, n, n, (float*)P_k);
-//Serial.println("Ending");
+math.MatrixMult((float*)temp2, (float*)P_k_1, n, n, n, (float*)P_k);
+Serial.println("Ending");
 }
diff --git a/main/main_kalman/main_kalman.pde b/main/main_kalman/main_kalman.pde
@@ -137,9 +137,11 @@ float R[m][m]={
     0,0,0,0,0,1  }
 };
 
+/* END OF THINGS TO TUNE */
+
+
 
 
-/* END OF THINGS TO TUNE */
 
 void MotorTest(void);
 
@@ -150,8 +152,8 @@ void MotorTest(void);
 // j = 1 -> Motor 2
 // k = 0 -> suffix _2 is the previous state for states
 // k = 1 -> suffix _1 is the previous state for states
-unsigned short int j;
-unsigned short int k;
+unsigned short int j = 0;
+unsigned short int k = 0;
 
 // Motor Directions
 unsigned int max_output = 200;
@@ -211,6 +213,34 @@ void test(float value[9],int pos)
 {
   //Serial.print(convert_to_dec(value[pos]));
 }
+void printstate(void)
+{
+    if(k == 0){
+        Serial.print(x_1_2[2], 4);
+        Serial.print(" ");
+        Serial.print(x_1_2[3], 4);
+        Serial.print(" ");
+    }
+    else{
+        Serial.print(x_1_1[2], 4);
+        Serial.print(" ");
+        Serial.print(x_1_1[3], 4);
+        Serial.print(" ");
+    }
+    if(k == 0){
+        Serial.print(x_2_2[2], 4);
+        Serial.print(" ");
+        Serial.println(x_2_2[3], 4);
+    }
+    else{
+        Serial.print(x_2_1[2], 4);
+        Serial.print(" ");
+        Serial.println(x_2_1[3], 4);
+    }
+}
+
+
+
 
 void setup()
 {
@@ -252,21 +282,50 @@ void loop() //Main Loop
     read_adc_offset();
     // actuate y-axis
     j = 0;
+    Serial.println("out of 0");
     KalmanFilter();
+    Serial.println("out of 1");
     StateSpaceControl();
+    Serial.println("out of 2");
     Actuate();
 
+    Serial.println("out of 3");
+
     // actuate x-axis
     j = 1;
     KalmanFilter();
+    Serial.println("out of 4");
     StateSpaceControl();
+    Serial.println("out of 5");
     Actuate();
+    Serial.println("out of 6");
 
 
 
     //print out the angle/vel of both axis
-    printstate();
-
+    /*if(k == 1000){
+      Serial.print("11111");
+        Serial.print(x_1_2[2], 4);
+        Serial.print(" ");
+        Serial.print(x_1_2[3], 4);
+        Serial.print(" ");
+    }
+    else{
+        Serial.print(x_1_1[2], 4);
+        Serial.print(" ");
+        Serial.print(x_1_1[3], 4);
+        Serial.print(" ");
+    }
+    if(k == 0){
+        Serial.print(x_2_2[2], 4);
+        Serial.print(" ");
+        Serial.println(x_2_2[3], 4);
+    }
+    else{
+        Serial.print(x_2_1[2], 4);
+        Serial.print(" ");
+        Serial.println(x_2_1[3], 4);
+    }*/
     //update which state stores the current state/covariance
     k = (k + 1) % 2;
   }
@@ -305,31 +364,7 @@ void loop() //Main Loop
 }
 
 
-void printstate(void)
-{
-    if(k == 0){
-        Serial.print(x_1_2[2], 4);
-        Serial.print(" ");
-        Serial.print(x_1_2[3], 4);
-        Serial.print(" ");
-    }
-    else{
-        Serial.print(x_1_1[2], 4);
-        Serial.print(" ");
-        Serial.print(x_1_1[3], 4);
-        Serial.print(" ");
-    }
-    if(k == 0){
-        Serial.print(x_2_2[2], 4);
-        Serial.print(" ");
-        Serial.println(x_2_2[3], 4);
-    }
-    else{
-        Serial.print(x_2_1[2], 4);
-        Serial.print(" ");
-        Serial.println(x_2_1[3], 4);
-    }
-}
+
 
 
 void printgains(void) {