Diff: main.cpp

Revision:

2:20d44bbe80e4

Parent:

0:c99eaed54c50

diff -r c99eaed54c50 -r 20d44bbe80e4 main.cpp
--- a/main.cpp	Mon Sep 27 09:02:40 2021 +0000
+++ b/main.cpp	Fri Nov 05 13:49:27 2021 +0000
@@ -1,13 +1,63 @@
 #include "mbed.h"
 #include "LSM9DS1.h"
 #include "LPS.h"
+#include "Matrix.h"
+#include <cmath>
+#include "MatrixMath.h"
 
-Serial pc(USBTX, USBRX, 57600);
-I2C i2c(PF_0, PF_1);
+Serial pc(USBTX, USBRX, 115200);
+I2C i2c(PB_9, PB_8);
 LSM9DS1 lsm(i2c);
 LPS lps(i2c);
 
-using namespace std;
+Matrix K(3,3);
+Matrix beta(3,1);
+float R;
+Matrix gamma(10,1);
+Matrix jac(10,1);
+float b;
+
+void setBetaK()
+{
+    K(1,1) = gamma(1,1);
+    K(1,2) = gamma(2,1);
+    K(2,1) = gamma(2,1);
+    K(1,3) = gamma(3,1);
+    K(3,1) = gamma(3,1);
+    K(2,2) = gamma(4,1);
+    K(2,3) = gamma(5,1);
+    K(3,2) = gamma(5,1);
+    K(3,3) = gamma(6,1);
+    beta(1,1) = gamma(7,1);
+    beta(2,1) = gamma(8,1);
+    beta(3,1) = gamma(9,1);
+    R = gamma(10,1);
+}
+
+void calcMagCalUpdate(Matrix H)
+{
+    float A = K(1,1)*(H(1,1)+beta(1,1)) +  K(1,2)*(H(2,1)+beta(2,1)) +   K(1,3)*(H(3,1)+beta(3,1)) ;
+    float B = K(2,1)*(H(1,1)+beta(1,1)) +  K(2,2)*(H(2,1)+beta(2,1)) +   K(2,3)*(H(3,1)+beta(3,1)) ;
+    float C = K(3,1)*(H(1,1)+beta(1,1)) +  K(3,2)*(H(2,1)+beta(2,1)) +   K(3,3)*(H(3,1)+beta(3,1)) ;
+    Matrix bvec = K * (H+beta);
+    b = sqrt(bvec(1,1)*bvec(1,1)+bvec(2,1)*bvec(2,1)+bvec(3,1)*bvec(3,1));
+    
+    jac(1,1) = - (H(1,1) + beta(1,1))*A/b;
+    jac(2,1) = - (H(2,1) + beta(2,1))*B/b;
+    jac(3,1) = - (H(3,1) + beta(3,1))*C/b;
+    jac(4,1) = - ((H(2,1) + beta(2,1))*A+(H(1,1) + beta(1,1))*B)/b;
+    jac(5,1) = - ((H(3,1) + beta(3,1))*A+(H(1,1) + beta(1,1))*C)/b;
+    jac(6,1) = - ((H(3,1) + beta(3,1))*B+(H(2,1) + beta(2,1))*C)/b;
+    
+    jac(7,1) = - (K(1,1)*(H(1,1) + beta(1,1))+K(1,2)*(H(2,1) + beta(2,1)) + K(1,3)*(H(3,1) + beta(3,1)))/b;
+    jac(8,1) = - (K(2,1)*(H(1,1) + beta(1,1))+K(2,2)*(H(2,1) + beta(2,1)) + K(2,3)*(H(3,1) + beta(3,1)))/b;
+    jac(9,1) = - (K(3,1)*(H(1,1) + beta(1,1))+K(3,2)*(H(2,1) + beta(2,1)) + K(3,3)*(H(3,1) + beta(3,1)))/b;
+    jac(10,1) = 1.0f;
+    
+    gamma = gamma - 0.001f*jac*(R-b);
+    //gamma = gamma - 0.001f*jac;
+
+}
 
 int main()
 {
@@ -18,20 +68,38 @@
         while (1);
     }
     lps.enableDefault();
+    
+    //初期値
+    R = 1.0f;
+    gamma(1,1) = 1.0f;
+    gamma(4,1) = 1.0f;
+    gamma(6,1) = 1.0f;
+    setBetaK();
+    
     wait_ms(100);
     while(true)
     {
         lsm.readAccel();
         lsm.readMag();
         lsm.readGyro();
-        //printf("%f %f %f %f %f %f %f %f %f\n", lsm.ax, lsm.ay, lsm.az, lsm.gx, lsm.gy, lsm.gz, lsm.mx, lsm.my, lsm.mz);
-        //printf("%f %f %f\n", lsm.gx, lsm.gy, lsm.gz);
-        //printf("%f %f %f\n", lsm.mx, lsm.my, lsm.mz);
+        //printf("%f %f %f %f %f %f %f %f %f\r\n", lsm.ax, lsm.ay, lsm.az, lsm.gx, lsm.gy, lsm.gz, lsm.mx, lsm.my, lsm.mz);
+        //printf("%f %f %f\r\n", lsm.gx, lsm.gy, lsm.gz);
+        //printf("%f %f %f\r\n", lsm.mx, lsm.my, lsm.mz);
         float pressure = lps.readPressureMillibars();
         float altitude = lps.pressureToAltitudeMeters(pressure);
         float temperature = lps.readTemperatureC();
+        Matrix h(3,1);
+        h <<  lsm.mx << lsm.my << lsm.mz;
+        Matrix hmod = K*(h+beta);
+        float res = R*R-(hmod(1,1)*hmod(1,1)+hmod(2,1)*hmod(2,1)+hmod(3,1)*hmod(3,1));
+        if(abs(res) >0.01f){
+            calcMagCalUpdate(h);
+            setBetaK();
+        }
+        
+
         //printf("p:%.2f\t mbar\ta:%.2f m\tt:%.2f deg C\r\n",pressure,altitude,temperature);
-        printf("%f %f %f %f %f %f %f %f %f %f\n", lsm.ax, lsm.ay, lsm.az, lsm.gx, lsm.gy, lsm.gz, lsm.mx, lsm.my, lsm.mz, pressure);
-        wait(0.1);
+        printf("%f %f %f %f %f %f %f %f %f %f %f %f %f %f\r\n", hmod(1,1),hmod(2,1),hmod(3,1),res,gamma(1,1),gamma(2,1),gamma(3,1),gamma(4,1),gamma(5,1),gamma(6,1),gamma(7,1),gamma(8,1),gamma(9,1),gamma(10,1));
+        wait(0.005);
     }
 }
\ No newline at end of file