Revision 2:dc7840a67f77, committed 2019-02-25

Comitter:

fbob

Date:

Mon Feb 25 16:39:52 2019 +0000

Parent:

1:085840a3d767

Commit message:

Update

Changed in this revision

--- a/AttitudeEstimator/AttitudeEstimator.cpp	Wed Feb 13 17:06:28 2019 +0000
+++ b/AttitudeEstimator/AttitudeEstimator.cpp	Mon Feb 25 16:39:52 2019 +0000
@@ -2,83 +2,209 @@
 #include "AttitudeEstimator.h"
 
 // Class constructor
-AttitudeEstimator::AttitudeEstimator() : x(4,1), z(3,1), A(4,4), H(3,4), P(4,4), K(4,3), Q(4,4), R(3,3), imu(A4,A5)
+AttitudeEstimator::AttitudeEstimator() : imu(A4,A5)
 {
 }
 
-// Initialize class 
+// Initialize class
 void AttitudeEstimator::init()
 {
     // Initialize IMU sensor object
     imu.init();
-    
-    dt = 0.01;
-    
-    x(1,1) = 1.0;
-    x(2,1) = 0.0;
-    x(3,1) = 0.0;
-    x(4,1) = 0.0;
+
+    dt = 0.005;
+    dt_half = dt/2.0;
     
-    P(1,1) = 1.0;
-    P(2,2) = 1.0;
-    P(3,3) = 1.0;
-    P(4,4) = 1.0;
+    x = eye(4,1);
+    A = eye(4);
+    P = eye(4);
+    Q = 0.001*eye(4);
+    R = 10.0*eye(4);
+    H = eye(4);
+    I = eye(4);
     
-    Q(1,1) = 1.0;
-    Q(2,2) = 1.0;
-    Q(3,3) = 1.0;
-    Q(4,4) = 1.0;
+    g = zeros(3,1);
+    a = zeros(3,1);
+    m = zeros(3,1);
     
-    R(1,1) = 100.0;
-    R(2,2) = 100.0;
-    R(3,3) = 100.0;
+    q = zeros(4,1);
     
-    
+    BN = eye(3);
+
 }
 
 // Estimate euler angles (rad) and angular velocity (rad/s)
 void AttitudeEstimator::estimate()
 {
     // Read IMU sensor data
-    imu.read();
+    read();
+    get_output();
+    
+    float omega_x = g(1,1)*dt_half;
+    float omega_y = g(2,1)*dt_half;
+    float omega_z = g(3,1)*dt_half;
     
-    A(1,2) = -imu.gx/2.0;
-    A(1,3) = -imu.gy/2.0;
-    A(1,4) = -imu.gz/2.0;
-    A(2,1) = imu.gx/2.0;
-    A(2,3) = imu.gz/2.0;
-    A(2,4) = -imu.gy/2.0;
-    A(3,1) = imu.gy/2.0;
-    A(3,2) = -imu.gz/2.0;
-    A(3,4) = imu.gx/2.0;
-    A(4,1) = imu.gz/2.0;
-    A(4,2) = imu.gy/2.0;
-    A(4,3) = -imu.gx/2.0;
+    /*A(1,1) = 1.0;
+    A(1,2) = -g(1,1)*dt/2.0;
+    A(1,3) = -g(2,1)*dt/2.0;
+    A(1,4) = -g(3,1)*dt/2.0;
+    A(2,1) = g(1,1)*dt/2.0;
+    A(2,2) = 1.0;
+    A(2,3) = g(3,1)*dt/2.0;
+    A(2,4) = -g(2,1)*dt/2.0;
+    A(3,1) = g(2,1)*dt/2.0;
+    A(3,2) = -g(3,1)*dt/2.0;
+    A(3,3) = 1.0;
+    A(3,4) = g(1,1)*dt/2.0;
+    A(4,1) = g(3,1)*dt/2.0;
+    A(4,2) = g(2,1)*dt/2.0;
+    A(4,3) = -g(1,1)*dt/2.0;
+    A(4,4) = 1.0;*/
     
-    H(1,1) = 2.0*x(3,1);
-    H(1,2) = -2.0*x(4,1);
-    H(1,3) = 2.0*x(1,1);
-    H(1,4) = -2.0*x(2,1);
-    H(2,1) = -2.0*x(2,1);
-    H(2,2) = -2.0*x(1,1);
-    H(2,3) = -2.0*x(4,1);
-    H(2,4) = -2.0*x(3,1);
-    H(3,1) = -2.0*x(1,1);
-    H(3,2) = 2.0*x(2,1);
-    H(3,3) = 2.0*x(3,1);
-    H(3,4) = -2.0*x(4,1);
+    A(1,2) = -omega_x;
+    A(1,3) = -omega_y;
+    A(1,4) = -omega_z;
+    A(2,1) = omega_x;
+    A(2,3) = omega_z;
+    A(2,4) = -omega_y;
+    A(3,1) = omega_y;
+    A(3,2) = -omega_z;
+    A(3,4) = omega_x;
+    A(4,1) = omega_z;
+    A(4,2) = omega_y;
+    A(4,3) = -omega_x;
+
+    x = A*x;
+    x = x/norm(x);
+
+    P = A*P*transpose(A)+Q;
+
+    //K = P*transpose(H)*inverse(H*P*transpose(H)+R);
+    K = P*inverse(P+R);
+
+    //x = x+K*(q-H*x);
+    x = x+K*(q-x);
+    x = x/norm(x);
+    //P = P-K*H*P;
+    P = P-K*P;
+    //P = P*(I-K*H);
+    //P = P-K*(H*P*transpose(H)+R)*transpose(K);
+    //P = (eye(4)-K*H)*P*transpose(eye(4)-K*H)+K*R*transpose(K);
+}
+
+void AttitudeEstimator::read()
+{
+    imu.read();
+    g(1,1) = imu.gx-0.0099;
+    g(2,1) = imu.gy+0.0693;
+    g(3,1) = imu.gz-0.0339;
+    a(1,1) = 1.0077*(imu.ax-0.0975);
+    a(2,1) = 1.0061*(imu.ay-0.0600);
+    a(3,1) = 0.9926*(imu.az-0.5156);
+    m(1,1) = 0.8838*(imu.mx+21.0631);
+    m(2,1) = 1.1537*(imu.my+8.9233);
+    m(3,1) = 0.9982*(imu.mz-11.8958);
+}
+
+void AttitudeEstimator::get_output()
+{
+    a = a/norm(a);
+    m = m/norm(m);
+
+    Matrix t1(3,1), t2(3,1), t3(3,1);
+    t1 = a;
+    //t2 = cross(a,m)/norm(cross(a,m));
+    t2 = cross(a,m);
+    t2 = t2/norm(t2);
+    t3 = cross(t1,t2);
+
+    /*Matrix BT(3,3), NT(3,3);
+    BT(1,1) = t1(1,1);
+    BT(2,1) = t1(2,1);
+    BT(3,1) = t1(3,1);
+    BT(1,2) = t2(1,1);
+    BT(2,2) = t2(2,1);
+    BT(3,2) = t2(3,1);
+    BT(1,3) = t3(1,1);
+    BT(2,3) = t3(2,1);
+    BT(3,3) = t3(3,1);
+
+    NT(1,2) = -0.3666;
+    NT(1,3) = -0.9304;
+    NT(2,2) = -0.9304;
+    NT(2,3) = 0.3666;
+    NT(3,1) = -1.0;
+
+    BN = BT*transpose(NT);*/
     
-    z(1,1) = imu.ax/9.81;
-    z(2,1) = imu.ay/9.81;
-    z(3,1) = imu.az/9.81;
     
-    x = x+A*x*dt;
-    x = x/norm(x);
-    P = A*P*transpose(A)+Q;
+    BN(1,1) = -t3(1,1);
+    BN(2,1) = -t3(2,1);
+    BN(3,1) = -t3(3,1);
+    BN(1,2) = -t2(1,1);
+    BN(2,2) = -t2(2,1);
+    BN(3,2) = -t2(3,1);
+    BN(1,3) = -t1(1,1);
+    BN(2,3) = -t1(2,1);
+    BN(3,3) = -t1(3,1);
+
+    /*q(4,1) =  0.0;
+
+    float tr = trace(BN);
+    if (tr > 0.0) {
+        float sqtrp1 = sqrt( tr + 1.0);
+        q(1,1) = 0.5*sqtrp1;
+        q(2,1) = (BN(2,3) - BN(3,2))/(2.0*sqtrp1);
+        q(3,1) = (BN(3,1) - BN(1,3))/(2.0*sqtrp1);
+        q(4,1) = (BN(1,2) - BN(2,1))/(2.0*sqtrp1);
+    } else {
+        if ((BN(2,2) > BN(1,1)) && (BN(2,2) > BN(3,3))) {
+            float sqdip1 = sqrt(BN(2,2) - BN(1,1) - BN(3,3) + 1.0 );
+            q(3,1) = 0.5*sqdip1;
+            if ( sqdip1 != 0 ) {
+                sqdip1 = 0.5/sqdip1;
+            }
+            q(1,1) = (BN(3,1) - BN(1,3))*sqdip1;
+            q(2,1) = (BN(1,2) + BN(2,1))*sqdip1;
+            q(4,1) = (BN(2,3) + BN(3,2))*sqdip1;
+        } else if (BN(3,3) > BN(1,1)) {
+            float sqdip1 = sqrt(BN(3,3) - BN(1,1) - BN(2,2) + 1.0 );
+            q(4,1) = 0.5*sqdip1;
+            if ( sqdip1 != 0 ) {
+                sqdip1 = 0.5/sqdip1;
+            }
+            q(1,1) = (BN(1,2) - BN(2,1))*sqdip1;
+            q(2,1) = (BN(3,1) + BN(1,3))*sqdip1;
+            q(3,1) = (BN(2,3) + BN(3,2))*sqdip1;
+        } else {
+            float sqdip1 = sqrt(BN(1,1) - BN(2,2) - BN(3,3) + 1.0 );
+            q(2,1) = 0.5*sqdip1;
+            if ( sqdip1 != 0 ) {
+                sqdip1 = 0.5/sqdip1;
+            }
+            q(1,1) = (BN(2,3) - BN(3,2))*sqdip1;
+            q(3,1) = (BN(1,2) + BN(2,1))*sqdip1;
+            q(4,1) = (BN(3,1) + BN(1,3))*sqdip1;
+        }
+    }*/
     
-    K = P*transpose(H)*inverse(H*P*transpose(H)+R);
-    
-    x = x+K*(z-H*x);
-    x = x/norm(x);
-    P = P-K*H*P; 
-}
\ No newline at end of file
+    q = dcm2quat(BN);
+
+    if((abs(x(1,1)) > abs(x(2,1))) && (abs(x(1,1)) > abs(x(3,1))) && (abs(x(1,1)) > abs(x(4,1)))) {
+        if (((x(1,1) > 0) && (q(1,1) < 0)) || ((x(1,1) < 0) && (q(1,1) > 0))) {
+            q = -q;
+        }
+    } else if ((abs(x(2,1)) > abs(x(3,1))) && (abs(x(2,1)) > abs(x(4,1)))) {
+        if (((x(2,1) > 0) && (q(2,1) < 0)) || ((x(2,1) < 0) && (q(2,1) > 0))) {
+            q = -q;
+        }
+    } else if ((abs(x(3,1)) > abs(x(4,1)))) {
+        if (((x(3,1) > 0) && (q(3,1) < 0)) || ((x(3,1) < 0) && (q(3,1) > 0))) {
+            q = -q;
+        }
+    } else {
+        if (((x(4,1) > 0) && (q(4,1) < 0)) || ((x(4,1) < 0) && (q(4,1) > 0))) {
+            q = -q;
+        }
+    }
+}

--- a/AttitudeEstimator/AttitudeEstimator.h	Wed Feb 13 17:06:28 2019 +0000
+++ b/AttitudeEstimator/AttitudeEstimator.h	Mon Feb 25 16:39:52 2019 +0000
@@ -16,14 +16,27 @@
     // Estimate Euler angles (rad) and angular velocities (rad/s)
     void estimate();
     //
+    void get_output();
+    //
     Matrix x;
     //
-    Matrix z, A, H, P, K, Q, R;
+    Matrix A, H, P, K, Q, R;
+    //
+    Matrix g, a, m;
+    //
+    Matrix q;
+    //
+    Matrix BN;
+    //
+    Matrix I;
   private:
     // 
-    float dt;
+    float dt, dt_half;
+    //
+    void read();
     // IMU sensor object
     LSM9DS1 imu;
+    //
 };
 
 #endif
\ No newline at end of file

--- a/LSM9DS1/LSM9DS1.cpp	Wed Feb 13 17:06:28 2019 +0000
+++ b/LSM9DS1/LSM9DS1.cpp	Mon Feb 25 16:39:52 2019 +0000
@@ -14,8 +14,9 @@
     // Test I2C bus
     if (test_i2c()) {
         // Setup accelerometer and gyroscope configurations
-        setup_accel();
-        setup_gyro();
+        setup_gyr();
+        setup_acc();
+        setup_mag();
         return true;
     } else {
         return false;
@@ -26,8 +27,9 @@
 void LSM9DS1::read()
 {
     // Read accelerometer and gyroscope data
-    read_accel();
-    read_gyro();
+    read_acc();
+    read_gyr();
+    read_mag();
 }
 
 /** Setup I2C bus */
@@ -39,68 +41,149 @@
 
 /** Test I2C bus */
 bool LSM9DS1::test_i2c()
-{
-    // Read device identity
-    uint8_t device_id = read_reg(WHO_AM_I);
-
-    // Check if device identity is 0x71
-    if (device_id == 0x68) {
+{   
+    // Register addresses
+    char reg_acc_gyr[1] = {WHO_AM_I};
+    char reg_mag[1] = {WHO_AM_I_M};
+    // Data that we're going to read
+    char data_acc_gyr[1];
+    char data_mag[1];
+    
+    // Point to register address
+    i2c.write(LSM9DS1_ADDRESS_ACC_GYR, reg_acc_gyr, 1);
+    // Read data from this address
+    i2c.read(LSM9DS1_ADDRESS_ACC_GYR, data_acc_gyr, 1);
+    
+    // Point to register address
+    i2c.write(LSM9DS1_ADDRESS_MAG, reg_mag, 1);
+    // Read data from this address
+    i2c.read(LSM9DS1_ADDRESS_MAG, data_mag, 1);
+    
+    // Check if device identity is 0x68 (acc/gyr) and 0x3D (mag)
+    if ((data_acc_gyr[0] == 0x68) && (data_mag[0] == 0x3D)) {
         return true;
     } else {
         return false;
     }
 }
 
-/** Setup accelerometer configurations (full-scale range) */
-void LSM9DS1::setup_accel(accel_scale a_scale)
+/** Setup gyroscope configurations (full-scale range) */
+void LSM9DS1::setup_gyr(gyr_scale g_scale)
 {
-    // Write configuration data
-    write_reg(CTRL_REG6_XL, (0b011 << 5) | (a_scale << 3) | 0b000);
+    // Register address and data that will be writed
+    char reg_data[2] = {CTRL_REG1_G, (0b011 << 5) | (g_scale << 3) | 0b000};
+    
+    // Point to register address and write data
+    i2c.write(LSM9DS1_ADDRESS_ACC_GYR, reg_data, 2);
+
+    // Adjust resolution [rad/s / bit] accordingly to choose scale
+    switch (g_scale) {
+        case GYR_SCALE_245DPS:
+            g_res = (245.0f*(PI/180.0f))/32768.0f;
+            break;
+        case GYR_SCALE_500DPS:
+            g_res = (500.0f*(PI/180.0f))/32768.0f;
+            break;
+        case GYR_SCALE_2000DPS:
+            g_res = (2000.0f*(PI/180.0f))/32768.0f;
+            break;
+    }
+}
+
+/** Setup accelerometer configurations (full-scale range) */
+void LSM9DS1::setup_acc(acc_scale a_scale)
+{
+    // Register address and data that will be writed
+    char reg_data[2] = {CTRL_REG6_XL, (0b011 << 5) | (a_scale << 3) | 0b000};
+    
+    // Point to register address and write data
+    i2c.write(LSM9DS1_ADDRESS_ACC_GYR, reg_data, 2);
 
     // Adjust resolution [m/s^2 / bit] accordingly to choose scale (g)
     switch (a_scale) {
-        case ACCEL_SCALE_2G:
+        case ACC_SCALE_2G:
             a_res = (2.0f*GRAVITY)/32768.0f;
             break;
-        case ACCEL_SCALE_4G:
+        case ACC_SCALE_4G:
             a_res = (4.0f*GRAVITY)/32768.0f;
             break;
-        case ACCEL_SCALE_8G:
+        case ACC_SCALE_8G:
             a_res = (8.0f*GRAVITY)/32768.0f;
             break;
-        case ACCEL_SCALE_16G:
+        case ACC_SCALE_16G:
             a_res = (16.0f*GRAVITY)/32768.0f;
             break;
     }
 }
 
 /** Setup gyroscope configurations (full-scale range) */
-void LSM9DS1::setup_gyro(gyro_scale g_scale)
+void LSM9DS1::setup_mag(mag_scale m_scale)
 {
-    // Write configuration data
-    write_reg(CTRL_REG1_G, (0b011 << 5) | (g_scale << 3) | 0b000);
+    // Register address and data that will be writed
+    /*char reg_data[2] = {CTRL_REG2_M, (0b0 << 7) | (m_scale << 5) | 0b00000};
+    
+    // Point to register address and write data
+    i2c.write(LSM9DS1_ADDRESS_MAG, reg_data, 2);*/
+    
+    char cmd[4] = {
+        CTRL_REG1_M,
+        0x10,       // Default data rate, xy axes mode, and temp comp
+        m_scale << 5, // Set mag scale
+        0           // Enable I2C, write only SPI, not LP mode, Continuous conversion mode
+    };
 
-    // Adjust resolution [rad/s / bit] accordingly to choose scale
-    switch (g_scale) {
-        case GYRO_SCALE_245DPS:
-            g_res = (245.0f*(PI/180.0f))/32768.0f;
+    // Write the data to the mag control registers
+    i2c.write(LSM9DS1_ADDRESS_MAG, cmd, 4);
+
+    // Adjust resolution [gauss / bit] accordingly to choosed scale
+    switch (m_scale) {
+        case MAG_SCALE_4G:
+            m_res = 400.0f/32768.0f;
             break;
-        case GYRO_SCALE_500DPS:
-            g_res = (500.0f*(PI/180.0f))/32768.0f;
+        case MAG_SCALE_8G:
+            m_res = 800.0f/32768.0f;
             break;
-        case GYRO_SCALE_2000DPS:
-            g_res = (2000.0f*(PI/180.0f))/32768.0f;
+        case MAG_SCALE_12G:
+            m_res = 1200.0f/32768.0f;
+            break;
+        case MAG_SCALE_16G:
+            m_res = 1600.0f/32768.0f;
             break;
     }
 }
 
-/** Read accelerometer output data */
-void LSM9DS1::read_accel()
+/** Read gyroscope data */
+void LSM9DS1::read_gyr()
 {
     // LSM9DS1 I2C bus address
-    char address = LSM9DS1_ADDRESS;
+    char address = LSM9DS1_ADDRESS_ACC_GYR;
     // Register address
-    char reg[1] = {OUT_X_XL_L};
+    char reg[1] = {OUT_X_L_G};
+    // Data that we're going to read
+    char data[6];
+
+    // Point to register address
+    i2c.write(address, reg, 1);
+    // Read data from this address (register address will auto-increment and all three axis information (two 8 bit data each) will be read)
+    i2c.read(address, data, 6);
+
+    // Reassemble the data (two 8 bit data into one 16 bit data)
+    int16_t gx_raw = data[0] | ( data[1] << 8 );
+    int16_t gy_raw = data[2] | ( data[3] << 8 );
+    int16_t gz_raw = data[4] | ( data[5] << 8 );
+    // Convert to SI units [rad/s]
+    gx = gx_raw * g_res;
+    gy = -gy_raw * g_res;
+    gz = gz_raw * g_res;
+}
+
+/** Read accelerometer output data */
+void LSM9DS1::read_acc()
+{
+    // LSM9DS1 I2C bus address
+    char address = LSM9DS1_ADDRESS_ACC_GYR;
+    // Register address
+    char reg[1] = {OUT_X_L_XL};
     // Data that we're going to read
     char data[6];
 
@@ -114,18 +197,18 @@
     int16_t ay_raw = data[2] | ( data[3] << 8 );
     int16_t az_raw = data[4] | ( data[5] << 8 );
     // Convert to SI units [m/s^2]
-    ax = -ay_raw * a_res;
-    ay = -ax_raw * a_res;
+    ax = -ax_raw * a_res;
+    ay = ay_raw * a_res;
     az = -az_raw * a_res;
 }
 
-/** Read gyroscope data */
-void LSM9DS1::read_gyro()
+/** Read magnetometer output data */
+void LSM9DS1::read_mag()
 {
     // LSM9DS1 I2C bus address
-    char address = LSM9DS1_ADDRESS;
+    char address = LSM9DS1_ADDRESS_MAG;
     // Register address
-    char reg[1] = {OUT_X_G_L};
+    char reg[1] = {OUT_X_L_M};
     // Data that we're going to read
     char data[6];
 
@@ -135,38 +218,11 @@
     i2c.read(address, data, 6);
 
     // Reassemble the data (two 8 bit data into one 16 bit data)
-    int16_t gx_raw = data[0] | ( data[1] << 8 );
-    int16_t gy_raw = data[2] | ( data[3] << 8 );
-    int16_t gz_raw = data[4] | ( data[5] << 8 );
-    // Convert to SI units [rad/s]
-    gx = gy_raw * g_res;
-    gy = gx_raw * g_res;
-    gz = gz_raw * g_res;
-}
-
-/** Write data into register on LSM9DS1 I2C bus address */
-void LSM9DS1::write_reg(uint8_t reg, uint8_t data)
-{
-    // Register address and data that will be writed
-    char i2c_reg_data[2] = {reg, data};
-
-    // Point to register address and write data
-    i2c.write(LSM9DS1_ADDRESS, i2c_reg_data, 2);
-}
-
-/** Read data from register on LSM9DS1 I2C bus address */
-char LSM9DS1::read_reg(uint8_t reg)
-{
-    // Register address
-    char i2c_reg[1] = {reg};
-    // Data that will be readed
-    char i2c_data[1];
-
-    // Point to register address
-    i2c.write(LSM9DS1_ADDRESS, i2c_reg, 1);
-    // Read data
-    i2c.read(LSM9DS1_ADDRESS, i2c_data, 1);
-
-    // Return readed data
-    return i2c_data[0];
+    int16_t mx_raw = data[0] | ( data[1] << 8 );
+    int16_t my_raw = data[2] | ( data[3] << 8 );
+    int16_t mz_raw = data[4] | ( data[5] << 8 );
+    // Convert to SI units [m/s^2]
+    mx = -mx_raw * m_res;
+    my = -my_raw * m_res;
+    mz = mz_raw * m_res;
 }
\ No newline at end of file

--- a/LSM9DS1/LSM9DS1.h	Wed Feb 13 17:06:28 2019 +0000
+++ b/LSM9DS1/LSM9DS1.h	Mon Feb 25 16:39:52 2019 +0000
@@ -8,46 +8,68 @@
 #define PI 3.14159f
 
 // LSM9DS1 I2C bus address
-#define LSM9DS1_ADDRESS 0xD6 
+#define LSM9DS1_ADDRESS_ACC_GYR 0x6B << 1 // (0xD6) Shift 1 bit left because mbed utilizes 8-bit addresses and not 7-bit 
+#define LSM9DS1_ADDRESS_MAG     0x1E << 1 // (0x3C) Shift 1 bit left because mbed utilizes 8-bit addresses and not 7-bit 
 
 // Device identity  
-#define WHO_AM_I 0x0F
+#define WHO_AM_I    0x0F
+#define WHO_AM_I_M  0x0F
+
+// Gyroscope configuration registers addresses
+#define CTRL_REG1_G 0x10
+// Gyroscope output register addresses
+#define OUT_X_L_G 0x18
+#define OUT_X_H_G 0x19
+#define OUT_Y_L_G 0x1A
+#define OUT_Y_H_G 0x1B
+#define OUT_Z_L_G 0x1C
+#define OUT_Z_H_G 0x1D
 
 // Accelerometer configuration registers addresses
 #define CTRL_REG6_XL 0x20
 // Accelerometer output register addresses
-#define OUT_X_XL_L 0x28
-#define OUT_X_XL_H 0x29
-#define OUT_Y_XL_L 0x2A
-#define OUT_Y_XL_H 0x2B
-#define OUT_Z_XL_L 0x2C
-#define OUT_Z_XL_H 0x2D
+#define OUT_X_L_XL 0x28
+#define OUT_X_H_XL 0x29
+#define OUT_Y_L_XL 0x2A
+#define OUT_Y_H_XL 0x2B
+#define OUT_Z_L_XL 0x2C
+#define OUT_Z_H_XL 0x2D
 
-// Gyroscope configuration registers addresses
-#define CTRL_REG1_G 0x10
-// Accelerometer output register addresses
-#define OUT_X_G_L 0x18
-#define OUT_X_G_H 0x19
-#define OUT_Y_G_L 0x1A
-#define OUT_Y_G_H 0x1B
-#define OUT_Z_G_L 0x1C
-#define OUT_Z_G_H 0x1D
+// Magnetometer configuration registers addresses
+#define CTRL_REG1_M 0x20
+#define CTRL_REG2_M 0x21
+// Magnetometer output register addresses
+#define OUT_X_L_M 0x28
+#define OUT_X_H_M 0x29
+#define OUT_Y_L_M 0x2A
+#define OUT_Y_H_M 0x2B
+#define OUT_Z_L_M 0x2C
+#define OUT_Z_H_M 0x2D
+
+// Gyroscope full-scale ranges
+enum gyr_scale
+{
+    GYR_SCALE_245DPS = 0b00,   
+    GYR_SCALE_500DPS = 0b01,  
+    GYR_SCALE_2000DPS = 0b11 
+};
 
 // Accelerometer full-scale ranges
-enum accel_scale
+enum acc_scale
 {
-    ACCEL_SCALE_2G = 0b00, 
-    ACCEL_SCALE_4G = 0b10,
-    ACCEL_SCALE_8G = 0b11, 
-    ACCEL_SCALE_16G = 0b01 
+    ACC_SCALE_2G = 0b00, 
+    ACC_SCALE_4G = 0b10,
+    ACC_SCALE_8G = 0b11, 
+    ACC_SCALE_16G = 0b01 
 };
 
-// Gyroscope full-scale ranges
-enum gyro_scale
+// Magnetometer full-scale ranges
+enum mag_scale
 {
-    GYRO_SCALE_245DPS = 0b00,   
-    GYRO_SCALE_500DPS = 0b01,  
-    GYRO_SCALE_2000DPS = 0b11 
+    MAG_SCALE_4G = 0b00,  
+    MAG_SCALE_8G = 0b01,
+    MAG_SCALE_12G = 0b10,
+    MAG_SCALE_16G = 0b11
 };
 
 /** LSM9DS1 (IMU sensor) class
@@ -67,8 +89,9 @@
  *     while(1)
  *     {
  *          imu.read();
- *          pc.printf("Accel  [m/s^2]: %6.2f %6.2f %6.2f \n", imu.ax, imu.ay, imu.az);
- *          pc.printf("Gyro   [rad/s]: %6.2f %6.2f %6.2f \n\n", imu.gx, imu.gy, imu.gz);
+            pc.printf("Acc [m/s^2]: %.1f | %.1f | %.1f \n", imu.ax, imu.ay, imu.az);
+            pc.printf("Gyr [rad/s]: %.1f | %.1f | %.1f \n", imu.gx, imu.gy, imu.gz);
+            pc.printf("Mag    [uT]: %.1f | %.1f | %.1f \n\n", imu.mx, imu.my, imu.mz);
  *          wait(0.2);
  *     }
  * }
@@ -78,6 +101,7 @@
 class LSM9DS1
 {
     public:
+    
         /** Class constructor */
         LSM9DS1(PinName sda, PinName scl);
         
@@ -86,19 +110,27 @@
         /** Read sensor data */
         void read();
         
+        /** Gyroscope data in x-axis [rad/s]*/
+        float gx;
+        /** Gyroscope data in y-axis [rad/s]*/
+        float gy;
+        /** Gyroscope data in z-axis [rad/s]*/
+        float gz;
         /** Accelerometer data in x-axis [m/s^2]*/
         float ax;
         /** Accelerometer data in y-axis [m/s^2]*/
         float ay;
         /** Accelerometer data in z-axis [m/s^2]*/
         float az;
-        /** Gyroscope data in x-axis [rad/s]*/
-        float gx;
-        /** Gyroscope data in y-axis [rad/s]*/
-        float gy;
-        /** Gyroscope data in z-axis [rad/s]*/
-        float gz;
+        /** Magnetometer data in x-axis [??]*/
+        float mx;
+        /** Magnetometer data in y-axis [??]*/
+        float my;
+        /** Magnetometer data in z-axis [??]*/
+        float mz;
+        
     private:
+    
         /** I2C bus */
         I2C i2c;
         
@@ -107,24 +139,26 @@
         /** Test I2C bus */
         bool test_i2c();
         
+        /** Setup gyroscope configurations (full-scale range) */
+        void setup_gyr(gyr_scale g_scale = GYR_SCALE_2000DPS);
         /** Setup accelerometer configurations (full-scale range) */
-        void setup_accel(accel_scale a_scale = ACCEL_SCALE_2G);
-        /** Setup gyroscope  configurations (full-scale range) */
-        void setup_gyro(gyro_scale g_scale = GYRO_SCALE_245DPS);
-        /** Read accelerometer data */
-        void read_accel();
+        void setup_acc(acc_scale a_scale = ACC_SCALE_2G);
+        /** Setup magnetometer configurations (full-scale range) */
+        void setup_mag(mag_scale m_scale = MAG_SCALE_4G);
+        
         /** Read gyroscope data */
-        void read_gyro();
+        void read_gyr();
+        /** Read accelerometer data */
+        void read_acc();
+        /** Read magnetometer data */
+        void read_mag();
         
-        /** Write data into register on LSM9DS1 I2C bus address */
-        void write_reg(uint8_t reg, uint8_t data);
-        /** Read data from register on LSM9DS1 I2C bus address */
-        char read_reg(uint8_t reg);
-        
+        /** Gyroscope resolution [rad/s / bit] */
+        float g_res;
         /** Accelerometer resolution [m/s^2 / bit] */
         float a_res;
-        /** Gyroscope resolution [rad/s / bit] */
-        float g_res;
+        /** Magnetometers resolution [uT / bit] */
+        float m_res;
         
 };
 

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MadgwickAHRS/MadgwickAHRS.cpp	Mon Feb 25 16:39:52 2019 +0000
@@ -0,0 +1,227 @@
+//=====================================================================================================
+// MadgwickAHRS.c
+//=====================================================================================================
+//
+// Implementation of Madgwick's IMU and AHRS algorithms.
+// See: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
+//
+// Date         Author          Notes
+// 29/09/2011   SOH Madgwick    Initial release
+// 02/10/2011   SOH Madgwick    Optimised for reduced CPU load
+// 19/02/2012   SOH Madgwick    Magnetometer measurement is normalised
+//
+//=====================================================================================================
+
+//---------------------------------------------------------------------------------------------------
+// Header files
+
+#include "MadgwickAHRS.h"
+#include <math.h>
+
+//---------------------------------------------------------------------------------------------------
+// Definitions
+
+#define sampleFreq  512.0f      // sample frequency in Hz
+#define betaDef     0.1f        // 2 * proportional gain
+
+//---------------------------------------------------------------------------------------------------
+// Variable definitions
+
+volatile float beta = betaDef;                              // 2 * proportional gain (Kp)
+volatile float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f;  // quaternion of sensor frame relative to auxiliary frame
+
+//---------------------------------------------------------------------------------------------------
+// Function declarations
+
+float invSqrt(float x);
+
+//====================================================================================================
+// Functions
+
+//---------------------------------------------------------------------------------------------------
+// AHRS algorithm update
+
+void MadgwickAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) {
+    float recipNorm;
+    float s0, s1, s2, s3;
+    float qDot1, qDot2, qDot3, qDot4;
+    float hx, hy;
+    float _2q0mx, _2q0my, _2q0mz, _2q1mx, _2bx, _2bz, _4bx, _4bz, _2q0, _2q1, _2q2, _2q3, _2q0q2, _2q2q3, q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;
+
+    // Use IMU algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation)
+    if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) {
+        MadgwickAHRSupdateIMU(gx, gy, gz, ax, ay, az);
+        return;
+    }
+
+    // Rate of change of quaternion from gyroscope
+    qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
+    qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);
+    qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);
+    qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);
+
+    // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
+    if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
+
+        // Normalise accelerometer measurement
+        recipNorm = invSqrt(ax * ax + ay * ay + az * az);
+        ax *= recipNorm;
+        ay *= recipNorm;
+        az *= recipNorm;   
+
+        // Normalise magnetometer measurement
+        recipNorm = invSqrt(mx * mx + my * my + mz * mz);
+        mx *= recipNorm;
+        my *= recipNorm;
+        mz *= recipNorm;
+
+        // Auxiliary variables to avoid repeated arithmetic
+        _2q0mx = 2.0f * q0 * mx;
+        _2q0my = 2.0f * q0 * my;
+        _2q0mz = 2.0f * q0 * mz;
+        _2q1mx = 2.0f * q1 * mx;
+        _2q0 = 2.0f * q0;
+        _2q1 = 2.0f * q1;
+        _2q2 = 2.0f * q2;
+        _2q3 = 2.0f * q3;
+        _2q0q2 = 2.0f * q0 * q2;
+        _2q2q3 = 2.0f * q2 * q3;
+        q0q0 = q0 * q0;
+        q0q1 = q0 * q1;
+        q0q2 = q0 * q2;
+        q0q3 = q0 * q3;
+        q1q1 = q1 * q1;
+        q1q2 = q1 * q2;
+        q1q3 = q1 * q3;
+        q2q2 = q2 * q2;
+        q2q3 = q2 * q3;
+        q3q3 = q3 * q3;
+
+        // Reference direction of Earth's magnetic field
+        hx = mx * q0q0 - _2q0my * q3 + _2q0mz * q2 + mx * q1q1 + _2q1 * my * q2 + _2q1 * mz * q3 - mx * q2q2 - mx * q3q3;
+        hy = _2q0mx * q3 + my * q0q0 - _2q0mz * q1 + _2q1mx * q2 - my * q1q1 + my * q2q2 + _2q2 * mz * q3 - my * q3q3;
+        _2bx = sqrt(hx * hx + hy * hy);
+        _2bz = -_2q0mx * q2 + _2q0my * q1 + mz * q0q0 + _2q1mx * q3 - mz * q1q1 + _2q2 * my * q3 - mz * q2q2 + mz * q3q3;
+        _4bx = 2.0f * _2bx;
+        _4bz = 2.0f * _2bz;
+
+        // Gradient decent algorithm corrective step
+        s0 = -_2q2 * (2.0f * q1q3 - _2q0q2 - ax) + _2q1 * (2.0f * q0q1 + _2q2q3 - ay) - _2bz * q2 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q3 + _2bz * q1) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q2 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
+        s1 = _2q3 * (2.0f * q1q3 - _2q0q2 - ax) + _2q0 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q1 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + _2bz * q3 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q2 + _2bz * q0) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q3 - _4bz * q1) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
+        s2 = -_2q0 * (2.0f * q1q3 - _2q0q2 - ax) + _2q3 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q2 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + (-_4bx * q2 - _2bz * q0) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q1 + _2bz * q3) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q0 - _4bz * q2) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
+        s3 = _2q1 * (2.0f * q1q3 - _2q0q2 - ax) + _2q2 * (2.0f * q0q1 + _2q2q3 - ay) + (-_4bx * q3 + _2bz * q1) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q0 + _2bz * q2) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q1 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
+        recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude
+        s0 *= recipNorm;
+        s1 *= recipNorm;
+        s2 *= recipNorm;
+        s3 *= recipNorm;
+
+        // Apply feedback step
+        qDot1 -= beta * s0;
+        qDot2 -= beta * s1;
+        qDot3 -= beta * s2;
+        qDot4 -= beta * s3;
+    }
+
+    // Integrate rate of change of quaternion to yield quaternion
+    q0 += qDot1 * (1.0f / sampleFreq);
+    q1 += qDot2 * (1.0f / sampleFreq);
+    q2 += qDot3 * (1.0f / sampleFreq);
+    q3 += qDot4 * (1.0f / sampleFreq);
+
+    // Normalise quaternion
+    recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
+    q0 *= recipNorm;
+    q1 *= recipNorm;
+    q2 *= recipNorm;
+    q3 *= recipNorm;
+}
+
+//---------------------------------------------------------------------------------------------------
+// IMU algorithm update
+
+void MadgwickAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az) {
+    float recipNorm;
+    float s0, s1, s2, s3;
+    float qDot1, qDot2, qDot3, qDot4;
+    float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 ,_8q1, _8q2, q0q0, q1q1, q2q2, q3q3;
+
+    // Rate of change of quaternion from gyroscope
+    qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
+    qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);
+    qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);
+    qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);
+
+    // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
+    if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
+
+        // Normalise accelerometer measurement
+        recipNorm = invSqrt(ax * ax + ay * ay + az * az);
+        ax *= recipNorm;
+        ay *= recipNorm;
+        az *= recipNorm;   
+
+        // Auxiliary variables to avoid repeated arithmetic
+        _2q0 = 2.0f * q0;
+        _2q1 = 2.0f * q1;
+        _2q2 = 2.0f * q2;
+        _2q3 = 2.0f * q3;
+        _4q0 = 4.0f * q0;
+        _4q1 = 4.0f * q1;
+        _4q2 = 4.0f * q2;
+        _8q1 = 8.0f * q1;
+        _8q2 = 8.0f * q2;
+        q0q0 = q0 * q0;
+        q1q1 = q1 * q1;
+        q2q2 = q2 * q2;
+        q3q3 = q3 * q3;
+
+        // Gradient decent algorithm corrective step
+        s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay;
+        s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * q1 - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az;
+        s2 = 4.0f * q0q0 * q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az;
+        s3 = 4.0f * q1q1 * q3 - _2q1 * ax + 4.0f * q2q2 * q3 - _2q2 * ay;
+        recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude
+        s0 *= recipNorm;
+        s1 *= recipNorm;
+        s2 *= recipNorm;
+        s3 *= recipNorm;
+
+        // Apply feedback step
+        qDot1 -= beta * s0;
+        qDot2 -= beta * s1;
+        qDot3 -= beta * s2;
+        qDot4 -= beta * s3;
+    }
+
+    // Integrate rate of change of quaternion to yield quaternion
+    q0 += qDot1 * (1.0f / sampleFreq);
+    q1 += qDot2 * (1.0f / sampleFreq);
+    q2 += qDot3 * (1.0f / sampleFreq);
+    q3 += qDot4 * (1.0f / sampleFreq);
+
+    // Normalise quaternion
+    recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
+    q0 *= recipNorm;
+    q1 *= recipNorm;
+    q2 *= recipNorm;
+    q3 *= recipNorm;
+}
+
+//---------------------------------------------------------------------------------------------------
+// Fast inverse square-root
+// See: http://en.wikipedia.org/wiki/Fast_inverse_square_root
+
+float invSqrt(float x) {
+    float halfx = 0.5f * x;
+    float y = x;
+    long i = *(long*)&y;
+    i = 0x5f3759df - (i>>1);
+    y = *(float*)&i;
+    y = y * (1.5f - (halfx * y * y));
+    return y;
+}
+
+//====================================================================================================
+// END OF CODE
+//====================================================================================================

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MadgwickAHRS/MadgwickAHRS.h	Mon Feb 25 16:39:52 2019 +0000
@@ -0,0 +1,31 @@
+//=====================================================================================================
+// MadgwickAHRS.h
+//=====================================================================================================
+//
+// Implementation of Madgwick's IMU and AHRS algorithms.
+// See: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
+//
+// Date         Author          Notes
+// 29/09/2011   SOH Madgwick    Initial release
+// 02/10/2011   SOH Madgwick    Optimised for reduced CPU load
+//
+//=====================================================================================================
+#ifndef MadgwickAHRS_h
+#define MadgwickAHRS_h
+
+//----------------------------------------------------------------------------------------------------
+// Variable declaration
+
+extern volatile float beta;             // algorithm gain
+extern volatile float q0, q1, q2, q3;   // quaternion of sensor frame relative to auxiliary frame
+
+//---------------------------------------------------------------------------------------------------
+// Function declarations
+
+void MadgwickAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz);
+void MadgwickAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az);
+
+#endif
+//=====================================================================================================
+// End of file
+//=====================================================================================================

--- a/Matrix/Matrix.cpp	Wed Feb 13 17:06:28 2019 +0000
+++ b/Matrix/Matrix.cpp	Mon Feb 25 16:39:52 2019 +0000
@@ -1,26 +1,27 @@
 #include "Matrix.h"
 
-Matrix::Matrix() : _rows(1), _cols(1)
+Matrix::Matrix()
 {
+    // Set matrix size
+    rows = 1;
+    cols = 1;
     // Allocate memory
-    _data = (float **)malloc(sizeof(float *));
-    _data[0] = (float *)malloc(sizeof(float));
-    // Initialize values
-    _data[0][0] = 0.0;
+    allocate_memmory();
+    // Initialize data
+    data[0][0] = 0.0;
 }
 
-Matrix::Matrix(int rows, int cols) : _rows(rows), _cols(cols)
+Matrix::Matrix(int r, int c)
 {
-
+    // Set matrix size
+    rows = r;
+    cols = c;
     // Allocate memory
-    _data = (float **)malloc(_rows * sizeof(float *)); // = new float*[_rows];
-    for (int i = 1; i <= _rows; i++) {
-        _data[i-1] = (float *)malloc(_cols * sizeof(float)); // = new float[_cols];
-    }
-    // Initialize values
-    for (int i = 1; i <= _rows; i++) {
-        for (int j = 1; j <= _cols; j++) {
-            _data[i-1][j-1] = 0.0;
+    allocate_memmory();
+    // Initialize data
+    for (int i = 0; i < rows; i++) {
+        for (int j = 0; j < cols; j++) {
+            data[i][j] = 0.0;
         }
     }
 }
@@ -28,55 +29,57 @@
 Matrix::~Matrix()
 {
     // Free memory
-    for (int i = 1; i <= _rows; i++) {
-        free(_data[i-1]);
-    }
-    free(_data);
+    deallocate_memmory();
 }
 
 Matrix& Matrix::operator=(const Matrix& m)
 {
-    for (int i = 1; i <= _rows; i++) {
-        for (int j = 1; j <= _cols; j++) {
-            _data[i-1][j-1] = m._data[i-1][j-1];
+    // Re-allocate memmory (in case size is different)
+    if (rows != m.rows || cols != m.cols) {
+        deallocate_memmory();
+        rows = m.rows;
+        cols = m.cols;
+        allocate_memmory();
+    }
+    // Copy data
+    for (int i = 0; i < rows; i++) {
+        for (int j = 0; j < cols; j++) {
+            data[i][j] = m.data[i][j];
         }
     }
     return *this;
 }
 
-float& Matrix::operator()(int row, int col)
+float& Matrix::operator()(int r, int c)
 {
     // Return cell data
-    return _data[row-1][col-1];
+    return data[r-1][c-1];
+}
+
+void Matrix::allocate_memmory()
+{
+    data = (float **)malloc(rows * sizeof(float *));
+    for (int i = 0; i < rows; i++) {
+        data[i] = (float *)malloc(cols * sizeof(float));
+    }
+}
+
+void Matrix::deallocate_memmory()
+{
+    for (int i = 0; i < rows; i++) {
+        free(data[i]);
+    }
+    free(data);
 }
 
 Matrix operator+(const Matrix& A, const Matrix& B)
 {
-    /*// Auxiliary matrix where C = A+B
-    Matrix C(A._rows,A._cols);
-    for (int i = 1; i <= A._rows; i++)
-    {
-      for (int j = 1; j <= A._cols; j++)
-      {
-          C._data[i-1][j-1] += A._data[i-1][j-1]+B._data[i-1][j-1];
-      }
-    }
-    return C;
-    */
-    
-    /*Matrix temp(m1);
-    return (temp += m2);
-    */
-    
-    //Matrix A(m1);
-    //Matrix B(m2);
-    Matrix C(A._rows,A._cols);
-    for (int i = 1; i <= A._rows; i++)
-    {
-      for (int j = 1; j <= A._cols; j++)
-      {
-          C._data[i-1][j-1] += A._data[i-1][j-1]+B._data[i-1][j-1];
-      }
+    // Auxiliary matrix where C = A+B
+    Matrix C(A.rows,A.cols);
+    for (int i = 0; i < C.rows; i++) {
+        for (int j = 0; j < C.cols; j++) {
+            C.data[i][j] = A.data[i][j]+B.data[i][j];
+        }
     }
     return C;
 }
@@ -84,10 +87,22 @@
 Matrix operator-(const Matrix& A, const Matrix& B)
 {
     // Auxiliary matrix where C = A-B
-    Matrix C(A._rows,A._cols);
-    for (int i = 1; i <= A._rows; i++) {
-        for (int j = 1; j <= A._cols; j++) {
-            C._data[i-1][j-1] += A._data[i-1][j-1]-B._data[i-1][j-1];
+    Matrix C(A.rows,A.cols);
+    for (int i = 0; i < C.rows; i++) {
+        for (int j = 0; j < C.cols; j++) {
+            C.data[i][j] = A.data[i][j]-B.data[i][j];
+        }
+    }
+    return C;
+}
+
+Matrix operator-(const Matrix& A)
+{
+    // Auxiliary matrix where C = -A
+    Matrix C(A.rows,A.cols);
+    for (int i = 0; i < C.rows; i++) {
+        for (int j = 0; j < C.cols; j++) {
+            C.data[i][j] = -A.data[i][j];
         }
     }
     return C;
@@ -96,109 +111,134 @@
 Matrix operator*(const Matrix& A, const Matrix& B)
 {
     // Auxiliary matrix where C = A*B
-    Matrix C(A._rows,B._cols);
-    for (int i = 1; i <= A._rows; i++) {
-        for (int j = 1; j <= B._cols; j++) {
-            for (int k = 1; k <= A._cols; k++) {
-                C._data[i-1][j-1] += A._data[i-1][k-1]*B._data[k-1][j-1];
+    Matrix C(A.rows,B.cols);
+    for (int i = 0; i < A.rows; i++) {
+        for (int j = 0; j < B.cols; j++) {
+            for (int k = 0; k < A.cols; k++) {
+                C.data[i][j] += A.data[i][k]*B.data[k][j];
             }
         }
     }
     return C;
 }
 
-Matrix operator*(float scalar, const Matrix& A)
+Matrix operator*(float k, const Matrix& A)
 {
-    // Auxiliary matrix where C = scalar*A
-    Matrix C(A._rows,A._cols);
-    for (int i = 1; i <= A._rows; i++) {
-        for (int j = 1; j <= A._cols; j++) {
-            C._data[i-1][j-1] += scalar*A._data[i-1][j-1];
+    // Auxiliary matrix where B = k*A
+    Matrix B(A.rows,A.cols);
+    for (int i = 0; i < B.rows; i++) {
+        for (int j = 0; j < B.cols; j++) {
+            B.data[i][j] = k*A.data[i][j];
         }
     }
-    return C;
+    return B;
+}
+
+Matrix operator*(const Matrix& A, float k)
+{
+    // Auxiliary matrix where B = A*k
+    Matrix B(A.rows,A.cols);
+    for (int i = 0; i < B.rows; i++) {
+        for (int j = 0; j < B.cols; j++) {
+            B.data[i][j] = A.data[i][j]*k;
+        }
+    }
+    return B;
 }
 
-Matrix operator*(const Matrix& A, float scalar)
+Matrix operator/(const Matrix& A, float k)
 {
-    // Auxiliary matrix where C = scalar*A
-    Matrix C(A._rows,A._cols);
-    for (int i = 1; i <= A._rows; i++) {
-        for (int j = 1; j <= A._cols; j++) {
-            C._data[i-1][j-1] += A._data[i-1][j-1]*scalar;
+    // Auxiliary matrix where B = A/k
+    Matrix B(A.rows,A.cols);
+    for (int i = 0; i < B.rows; i++) {
+        for (int j = 0; j < B.cols; j++) {
+            B.data[i][j] = A.data[i][j]/k;
         }
     }
-    return C;
+    return B;
 }
 
-Matrix operator/(const Matrix& A, float scalar)
+Matrix eye(int r, int c)
 {
-    // Auxiliary matrix where C = scalar*A
-    Matrix C(A._rows,A._cols);
-    for (int i = 1; i <= A._rows; i++) {
-        for (int j = 1; j <= A._cols; j++) {
-            C._data[i-1][j-1] += A._data[i-1][j-1]/scalar;
+    if (c == 0) {
+        c = r;
+    }
+    Matrix m(r, c);
+    for (int i = 0; i < m.rows; i++) {
+        for (int j = 0; j < m.cols; j++) {
+            if (i == j) {
+                m.data[i][j] = 1.0;
+            }
         }
     }
-    return C;
+    return m;
+}
+
+Matrix zeros(int r, int c)
+{
+    if (c == 0) {
+        c = r;
+    }
+    Matrix m(r, c);
+    return m;
 }
 
 Matrix transpose(const Matrix& A)
 {
-    // Auxiliary matrix where C = A'
-    Matrix C(A._cols, A._rows);
-    for (int i = 1; i <= C._rows; i++) {
-        for (int j = 1; j <= C._cols; j++) {
-            C._data[i-1][j-1] = A._data[j-1][i-1];
+    // Auxiliary matrix where B = A'
+    Matrix B(A.cols, A.rows);
+    for (int i = 0; i < B.rows; i++) {
+        for (int j = 0; j < B.cols; j++) {
+            B.data[i][j] = A.data[j][i];
         }
     }
-    return C;
+    return B;
 }
 
 Matrix inverse(const Matrix& A)
 {
     // Apply A = LDL' factorization where L is a lower triangular matrix and D
     // is a block diagonal matrix
-    Matrix L(A._cols, A._rows);
-    Matrix D(A._cols, A._rows);
+    Matrix L(A.rows, A.cols);
+    Matrix D(A.rows, A.cols);
     float L_sum;
     float D_sum;
-    for (int i = 1; i <= A._rows; i++) {
-        for (int j = 1; j <= A._rows; j++) {
+    for (int i = 0; i < A.rows; i++) {
+        for (int j = 0; j < A.rows; j++) {
             if (i >= j) {
                 if (i == j) {
-                    L._data[i-1][j-1] = 1.0;
+                    L.data[i][j] = 1.0;
                     D_sum = 0.0;
-                    for (int k = 1; k <= (i-1); k++) {
-                        D_sum += L._data[i-1][k-1]*L._data[i-1][k-1]*D._data[k-1][k-1];
+                    for (int k = 0; k <= (i-1); k++) {
+                        D_sum += L.data[i][k]*L.data[i][k]*D.data[k][k];
                     }
-                    D._data[i-1][j-1] = A._data[i-1][j-1] - D_sum;
+                    D.data[i][j] = A.data[i][j] - D_sum;
                 } else {
                     L_sum = 0.0;
-                    for (int k = 1; k <= (j-1); k++) {
-                        L_sum += L._data[i-1][k-1]*L._data[j-1][k-1]*D._data[k-1][k-1];
+                    for (int k = 0; k <= (j-1); k++) {
+                        L_sum += L.data[i][k]*L.data[j][k]*D.data[k][k];
                     }
-                    L._data[i-1][j-1] = (1.0/D._data[j-1][j-1])*(A._data[i-1][j-1]-L_sum);
+                    L.data[i][j] = (1.0/D.data[j][j])*(A.data[i][j]-L_sum);
                 }
             }
         }
     }
     // Compute the inverse of L and D matrices
-    Matrix L_inv(A._cols, A._rows);
-    Matrix D_inv(A._cols, A._rows);
+    Matrix L_inv(A.rows, A.cols);
+    Matrix D_inv(A.rows, A.cols);
     float L_inv_sum;
-    for (int i = 1; i <= A._rows; i++) {
-        for (int j = 1; j <= A._rows; j++) {
+    for (int i = 0; i < A.rows; i++) {
+        for (int j = 0; j < A.rows; j++) {
             if (i >= j) {
                 if (i == j) {
-                    L_inv._data[i-1][j-1] = 1.0/L._data[i-1][j-1];
-                    D_inv._data[i-1][j-1] = 1.0/D._data[i-1][j-1];
+                    L_inv.data[i][j] = 1.0/L.data[i][j];
+                    D_inv.data[i][j] = 1.0/D.data[i][j];
                 } else {
                     L_inv_sum = 0.0;
                     for (int k = j; k <= (i-1); k++) {
-                        L_inv_sum += L._data[i-1][k-1]*L_inv._data[k-1][j-1];
+                        L_inv_sum += L.data[i][k]*L_inv.data[k][j];
                     }
-                    L_inv._data[i-1][j-1] = -L_inv_sum;
+                    L_inv.data[i][j] = -L_inv_sum;
                 }
             }
         }
@@ -208,13 +248,73 @@
     return transpose(L_inv)*D_inv*L_inv;
 }
 
+float trace(const Matrix& A)
+{
+    float t = 0.0;
+    for (int i = 0; i < A.rows; i++) {
+        t += A.data[i][i];
+    }
+    return t;
+}
+
+Matrix cross(const Matrix& u, const Matrix& v)
+{
+    // Auxiliary matrix where w = uXv
+    Matrix w(u.rows,u.cols);
+    w.data[0][0] = u.data[1][0]*v.data[2][0]-u.data[2][0]*v.data[1][0];
+    w.data[1][0] = u.data[2][0]*v.data[0][0]-u.data[0][0]*v.data[2][0];
+    w.data[2][0] = u.data[0][0]*v.data[1][0]-u.data[1][0]*v.data[0][0];
+    return w;
+}
+
 float norm(const Matrix& A)
 {
-    // Auxiliary matrix where C = A'
-    float n;
-    n = 0.0;
-    for (int i = 1; i <= A._rows; i++) {
-        n += A._data[i-1][0]*A._data[i-1][0];
-        }
+    float n = 0.0;
+    for (int i = 0; i < A.rows; i++) {
+        n += A.data[i][0]*A.data[i][0];
+    }
     return sqrt(n);
 }
+
+Matrix dcm2quat(const Matrix& R)
+{
+    Matrix q(4, 1);
+    float tr = trace(R);
+    if (tr > 0.0) {
+        float sqtrp1 = sqrt( tr + 1.0);
+        q.data[0][0] = 0.5*sqtrp1;
+        q.data[1][0] = (R.data[1][2] - R.data[2][1])/(2.0*sqtrp1);
+        q.data[2][0] = (R.data[2][0] - R.data[0][2])/(2.0*sqtrp1);
+        q.data[3][0] = (R.data[0][1] - R.data[1][0])/(2.0*sqtrp1);
+    } else {
+        if ((R.data[1][1] > R.data[0][0]) && (R.data[1][1] > R.data[2][2])) {
+            float sqdip1 = sqrt(R.data[1][1] - R.data[0][0] - R.data[2][2] + 1.0 );
+            q.data[2][0] = 0.5*sqdip1;
+            if ( sqdip1 != 0 ) {
+                sqdip1 = 0.5/sqdip1;
+            }
+            q.data[0][0] = (R.data[2][0] - R.data[0][2])*sqdip1;
+            q.data[1][0] = (R.data[0][1] + R.data[1][0])*sqdip1;
+            q.data[3][0] = (R.data[1][2] + R.data[2][1])*sqdip1;
+        } else if (R.data[2][2] > R.data[0][0]) {
+            float sqdip1 = sqrt(R.data[2][2] - R.data[0][0] - R.data[1][1] + 1.0 );
+            q.data[3][0] = 0.5*sqdip1;
+            if ( sqdip1 != 0 ) {
+                sqdip1 = 0.5/sqdip1;
+            }
+            q.data[0][0] = (R.data[0][1] - R.data[1][0])*sqdip1;
+            q.data[1][0] = (R.data[2][0] + R.data[0][2])*sqdip1;
+            q.data[2][0] = (R.data[1][2] + R.data[2][1])*sqdip1;
+        } else {
+            float sqdip1 = sqrt(R.data[0][0] - R.data[1][1] - R.data[2][2] + 1.0 );
+            q.data[1][0] = 0.5*sqdip1;
+            if ( sqdip1 != 0 ) {
+                sqdip1 = 0.5/sqdip1;
+            }
+            q.data[0][0] = (R.data[1][2] - R.data[2][1])*sqdip1;
+            q.data[2][0] = (R.data[0][1] + R.data[1][0])*sqdip1;
+            q.data[3][0] = (R.data[2][0] + R.data[0][2])*sqdip1;
+        }
+    }
+    return q;
+}
\ No newline at end of file

--- a/Matrix/Matrix.h	Wed Feb 13 17:06:28 2019 +0000
+++ b/Matrix/Matrix.h	Mon Feb 25 16:39:52 2019 +0000
@@ -7,32 +7,46 @@
 /** Matrix class */
 class Matrix
 {
-  public:
+public:
     /** Constructors */
     Matrix();
-    Matrix(int rows, int cols);
+    Matrix(int, int);
     /** Destructor */
     ~Matrix();
     /** Assignment */
-    Matrix& operator=(const Matrix& m);
+    Matrix& operator=(const Matrix&);
+    /** Cell data */
+    float& operator()(int, int);
     /** Parameters */
-    int _rows, _cols;
-    float **_data;
-    /** Cell data */
-    float& operator()(int row, int col);
+    int rows, cols;
+    float **data;
+private:
+    /** Memmory managment **/
+    void allocate_memmory();
+    void deallocate_memmory();
 };
 
 /** Math operators */
 Matrix operator+(const Matrix&, const Matrix&);
 Matrix operator-(const Matrix&, const Matrix&);
+Matrix operator-(const Matrix&);
 Matrix operator*(const Matrix&, const Matrix&);
 Matrix operator*(float, const Matrix&);
 Matrix operator*(const Matrix&, float);
 Matrix operator/(const Matrix&, float);
 
 /** Matriz algebra */
+Matrix eye(int, int = 0);
+Matrix zeros(int, int = 0);
 Matrix transpose(const Matrix&);
 Matrix inverse(const Matrix&);
+float trace(const Matrix&);
+
+/** Vector algebra **/
+Matrix cross(const Matrix&, const Matrix&);
 float norm(const Matrix&);
 
+/** Orientation algebra **/
+Matrix dcm2quat(const Matrix&);
+
 #endif