Diff: Calibration/CALIBRATION.cpp

Revision:

8:9e6ead2ee8d7

Child:

9:6c803986dbde

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Calibration/CALIBRATION.cpp	Tue Aug 21 13:25:37 2018 +0000
@@ -0,0 +1,129 @@
+#include <stdint.h>
+#include <stdio.h>
+#include <math.h>
+
+#include "mbed.h"
+#include "CALIBRATION.h"
+
+
+#define PI 3.14159265
+
+CALIBRATION::calib_data_t CALIBRATION::convert_2p(float angle[11][2], float meas[11][2]){
+    calib_data_t  res;
+    for(int i=0; i<3; i++){ 
+        res.S[i][i]= (angle[i][1]-angle[i][0])/(meas[i][1]-meas[i][0]);
+        res.B[i][0]= (angle[i][0]*meas[i][1]-angle[i][1]*meas[i][0])/(meas[i][1]-meas[i][0]);
+    }
+    return res;
+}
+CALIBRATION::calib_data_t CALIBRATION::convert_6p(float angle[11][2], float meas[11][2]){
+    calib_data_t  res;
+    for(int i=0; i<3; i++){ 
+        res.S[i][i]= (angle[i][1]-angle[i][0])/(meas[i][1]-meas[i][0]);
+        res.B[i][0]= (angle[i][0]*meas[i][1]-angle[i][1]*meas[i][0])/(meas[i][1]-meas[i][0]);
+    }
+    return res;
+}
+//CALIBRATION::calib_data_t CALIBRATION::convert_12p(float angle[11][2], float meas[11][2], int size){}
+
+
+/*For calculating Determinant of the Matrix */
+
+void CALIBRATION::matrix_reset(){
+    memset(g_matrix, 0, sizeof(g_matrix[0][0]) * 2 * 11);
+    memset(coeff_matrix.x_matrix, 0, sizeof(coeff_matrix.x_matrix[0][0]) * 11 * 3);  
+    memset(w_matrix, 0, sizeof(w_matrix[0][0]) * 3 * 2);
+    }
+
+
+void CALIBRATION::matrix_g(float angle[4][12]){
+    // angle expressed in degree: roll, pitch, yaw
+    double roll, pitch;
+    int i;
+    for ( i = 0; i < 12; ++i) {
+        roll = angle[0][i]*PI/180;
+        pitch = angle[1][i]*PI/180;
+        g_matrix[0][i] = -sin(pitch);
+        g_matrix[1][i] = cos(pitch)*sin(roll);
+        g_matrix[2][i] = cos(pitch)*cos(roll);
+        }
+   }
+
+void CALIBRATION::matrix_x(float meas[3][12]){
+    int i,j,k;
+    // define xt and x
+    for( i = 0; i < 12; i++){
+        coeff_matrix.x_matrix[i][0] = meas[0][i];
+        coeff_matrix.x_matrix[i][1] = meas[1][i];
+        coeff_matrix.x_matrix[i][2] = meas[2][i];
+        coeff_matrix.x_matrix[i][3] = 1;
+        coeff_matrix.x_transpose[0][i] = meas[0][i];
+        coeff_matrix.x_transpose[1][i] = meas[1][i];
+        coeff_matrix.x_transpose[2][i] = meas[2][i];
+        coeff_matrix.x_transpose[3][i] = 1;
+    }
+    // product xt * x
+    double sum = 0;
+    for ( i=0; i< 12; i++){
+        for( j=0; j<3; j++){
+            sum = sum + coeff_matrix.x_transpose[i][j]+coeff_matrix.x_matrix[i][j];
+            }
+        coeff_matrix.xtx_product[i][j] = sum;
+        sum = 0;
+        }
+    // determinte  of xt*t
+    float det;
+    det = matrix_determinant(coeff_matrix.xtx_product,12);
+    // cofactor of ct*x
+    float b[12][12],fac[12][12];
+    int p,q;
+    int m,n;
+    for (q=0;q<12;q++){
+        for (p=0;p<12;p++){
+            m=0; n=0;
+            for (i=0;i<12;i++){
+                for (j=0;j<12;j++){
+                    if (i != q && j != p){
+                        b[m][n]=coeff_matrix.xtx_product[i][j];
+                        if (n<(12-2))
+                            n++;
+                        else{
+                            n=0; m++;}
+                    }
+                }
+            }
+            //fac[q][p] = pow(-1,p+q)* matrix_determinant(b,12-1);
+        }
+    }
+}
+
+float CALIBRATION::matrix_determinant(float a[12][12], float k){
+  float s = 1, det = 0, b[12][12];
+  int i, j, m, n, c;
+  if (k == 1) { return (a[0][0]);}
+  else{
+     det = 0;
+     for (c = 0; c < k; c++){
+        m = 0; n = 0;
+        for (i = 0;i < k; i++){
+            for (j = 0 ;j < k; j++){
+                b[i][j] = 0;
+                if (i != 0 && j != c){
+                   b[m][n] = a[i][j];
+                   if (n < (k - 2))
+                    n++;
+                   else
+                    {n = 0; m++;}
+                   }
+               }
+             }
+          det = det + s * (a[0][c] * matrix_determinant(b, k - 1));
+          s = -1 * s;
+          }
+    }
+    return (det);
+}
+ 
+//https://www.sanfoundry.com/c-program-find-inverse-matrix/
+
+//http://www.ccodechamp.com/c-program-to-find-inverse-of-matrix/
\ No newline at end of file