File content as of revision 2:20d44bbe80e4:

#include "mbed.h"
#include "LSM9DS1.h"
#include "LPS.h"
#include "Matrix.h"
#include <cmath>
#include "MatrixMath.h"

Serial pc(USBTX, USBRX, 115200);
I2C i2c(PB_9, PB_8);
LSM9DS1 lsm(i2c);
LPS lps(i2c);

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()
{
    uint16_t reg = lsm.begin();
    //printf("%x\n", reg);
    if (!lps.init()){
        printf("Failed to autodetect pressure sensor!\r\n");
        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\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 %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);
    }
}