Fabio Bobrow
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
--- 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