Cubli library
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
diff -r 085840a3d767 -r dc7840a67f77 AttitudeEstimator/AttitudeEstimator.cpp --- 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; + } + } +}
diff -r 085840a3d767 -r dc7840a67f77 AttitudeEstimator/AttitudeEstimator.h --- 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
diff -r 085840a3d767 -r dc7840a67f77 LSM9DS1/LSM9DS1.cpp --- 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
diff -r 085840a3d767 -r dc7840a67f77 LSM9DS1/LSM9DS1.h --- 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; };
diff -r 085840a3d767 -r dc7840a67f77 MadgwickAHRS/MadgwickAHRS.cpp --- /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 +//====================================================================================================
diff -r 085840a3d767 -r dc7840a67f77 MadgwickAHRS/MadgwickAHRS.h --- /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 +//=====================================================================================================
diff -r 085840a3d767 -r dc7840a67f77 Matrix/Matrix.cpp --- 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
diff -r 085840a3d767 -r dc7840a67f77 Matrix/Matrix.h --- 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