Fabio Bobrow / Cubli

Cubli library

AttitudeEstimator/AttitudeEstimator.cpp@2:dc7840a67f77, 2019-02-25 (annotated)

Committer:
fbob
Date:
Mon Feb 25 16:39:52 2019 +0000
Revision:
2:dc7840a67f77
Parent:
1:085840a3d767 
Update

Who changed what in which revision?

UserRevisionLine numberNew contents of line
fbob 1:085840a3d767 1 #include "mbed.h"
fbob 1:085840a3d767 2 #include "AttitudeEstimator.h"
fbob 1:085840a3d767 3
fbob 1:085840a3d767 4 // Class constructor
fbob 2:dc7840a67f77 5 AttitudeEstimator::AttitudeEstimator() : imu(A4,A5)
fbob 1:085840a3d767 6 {
fbob 1:085840a3d767 7 }
fbob 1:085840a3d767 8
fbob 2:dc7840a67f77 9 // Initialize class
fbob 1:085840a3d767 10 void AttitudeEstimator::init()
fbob 1:085840a3d767 11 {
fbob 1:085840a3d767 12 // Initialize IMU sensor object
fbob 1:085840a3d767 13 imu.init();
fbob 2:dc7840a67f77 14
fbob 2:dc7840a67f77 15 dt = 0.005;
fbob 2:dc7840a67f77 16 dt_half = dt/2.0;
fbob 1:085840a3d767 17
fbob 2:dc7840a67f77 18 x = eye(4,1);
fbob 2:dc7840a67f77 19 A = eye(4);
fbob 2:dc7840a67f77 20 P = eye(4);
fbob 2:dc7840a67f77 21 Q = 0.001*eye(4);
fbob 2:dc7840a67f77 22 R = 10.0*eye(4);
fbob 2:dc7840a67f77 23 H = eye(4);
fbob 2:dc7840a67f77 24 I = eye(4);
fbob 1:085840a3d767 25
fbob 2:dc7840a67f77 26 g = zeros(3,1);
fbob 2:dc7840a67f77 27 a = zeros(3,1);
fbob 2:dc7840a67f77 28 m = zeros(3,1);
fbob 1:085840a3d767 29
fbob 2:dc7840a67f77 30 q = zeros(4,1);
fbob 1:085840a3d767 31
fbob 2:dc7840a67f77 32 BN = eye(3);
fbob 2:dc7840a67f77 33
fbob 1:085840a3d767 34 }
fbob 1:085840a3d767 35
fbob 1:085840a3d767 36 // Estimate euler angles (rad) and angular velocity (rad/s)
fbob 1:085840a3d767 37 void AttitudeEstimator::estimate()
fbob 1:085840a3d767 38 {
fbob 1:085840a3d767 39 // Read IMU sensor data
fbob 2:dc7840a67f77 40 read();
fbob 2:dc7840a67f77 41 get_output();
fbob 2:dc7840a67f77 42
fbob 2:dc7840a67f77 43 float omega_x = g(1,1)*dt_half;
fbob 2:dc7840a67f77 44 float omega_y = g(2,1)*dt_half;
fbob 2:dc7840a67f77 45 float omega_z = g(3,1)*dt_half;
fbob 1:085840a3d767 46
fbob 2:dc7840a67f77 47 /*A(1,1) = 1.0;
fbob 2:dc7840a67f77 48 A(1,2) = -g(1,1)*dt/2.0;
fbob 2:dc7840a67f77 49 A(1,3) = -g(2,1)*dt/2.0;
fbob 2:dc7840a67f77 50 A(1,4) = -g(3,1)*dt/2.0;
fbob 2:dc7840a67f77 51 A(2,1) = g(1,1)*dt/2.0;
fbob 2:dc7840a67f77 52 A(2,2) = 1.0;
fbob 2:dc7840a67f77 53 A(2,3) = g(3,1)*dt/2.0;
fbob 2:dc7840a67f77 54 A(2,4) = -g(2,1)*dt/2.0;
fbob 2:dc7840a67f77 55 A(3,1) = g(2,1)*dt/2.0;
fbob 2:dc7840a67f77 56 A(3,2) = -g(3,1)*dt/2.0;
fbob 2:dc7840a67f77 57 A(3,3) = 1.0;
fbob 2:dc7840a67f77 58 A(3,4) = g(1,1)*dt/2.0;
fbob 2:dc7840a67f77 59 A(4,1) = g(3,1)*dt/2.0;
fbob 2:dc7840a67f77 60 A(4,2) = g(2,1)*dt/2.0;
fbob 2:dc7840a67f77 61 A(4,3) = -g(1,1)*dt/2.0;
fbob 2:dc7840a67f77 62 A(4,4) = 1.0;*/
fbob 1:085840a3d767 63
fbob 2:dc7840a67f77 64 A(1,2) = -omega_x;
fbob 2:dc7840a67f77 65 A(1,3) = -omega_y;
fbob 2:dc7840a67f77 66 A(1,4) = -omega_z;
fbob 2:dc7840a67f77 67 A(2,1) = omega_x;
fbob 2:dc7840a67f77 68 A(2,3) = omega_z;
fbob 2:dc7840a67f77 69 A(2,4) = -omega_y;
fbob 2:dc7840a67f77 70 A(3,1) = omega_y;
fbob 2:dc7840a67f77 71 A(3,2) = -omega_z;
fbob 2:dc7840a67f77 72 A(3,4) = omega_x;
fbob 2:dc7840a67f77 73 A(4,1) = omega_z;
fbob 2:dc7840a67f77 74 A(4,2) = omega_y;
fbob 2:dc7840a67f77 75 A(4,3) = -omega_x;
fbob 2:dc7840a67f77 76
fbob 2:dc7840a67f77 77 x = A*x;
fbob 2:dc7840a67f77 78 x = x/norm(x);
fbob 2:dc7840a67f77 79
fbob 2:dc7840a67f77 80 P = A*P*transpose(A)+Q;
fbob 2:dc7840a67f77 81
fbob 2:dc7840a67f77 82 //K = P*transpose(H)*inverse(H*P*transpose(H)+R);
fbob 2:dc7840a67f77 83 K = P*inverse(P+R);
fbob 2:dc7840a67f77 84
fbob 2:dc7840a67f77 85 //x = x+K*(q-H*x);
fbob 2:dc7840a67f77 86 x = x+K*(q-x);
fbob 2:dc7840a67f77 87 x = x/norm(x);
fbob 2:dc7840a67f77 88 //P = P-K*H*P;
fbob 2:dc7840a67f77 89 P = P-K*P;
fbob 2:dc7840a67f77 90 //P = P*(I-K*H);
fbob 2:dc7840a67f77 91 //P = P-K*(H*P*transpose(H)+R)*transpose(K);
fbob 2:dc7840a67f77 92 //P = (eye(4)-K*H)*P*transpose(eye(4)-K*H)+K*R*transpose(K);
fbob 2:dc7840a67f77 93 }
fbob 2:dc7840a67f77 94
fbob 2:dc7840a67f77 95 void AttitudeEstimator::read()
fbob 2:dc7840a67f77 96 {
fbob 2:dc7840a67f77 97 imu.read();
fbob 2:dc7840a67f77 98 g(1,1) = imu.gx-0.0099;
fbob 2:dc7840a67f77 99 g(2,1) = imu.gy+0.0693;
fbob 2:dc7840a67f77 100 g(3,1) = imu.gz-0.0339;
fbob 2:dc7840a67f77 101 a(1,1) = 1.0077*(imu.ax-0.0975);
fbob 2:dc7840a67f77 102 a(2,1) = 1.0061*(imu.ay-0.0600);
fbob 2:dc7840a67f77 103 a(3,1) = 0.9926*(imu.az-0.5156);
fbob 2:dc7840a67f77 104 m(1,1) = 0.8838*(imu.mx+21.0631);
fbob 2:dc7840a67f77 105 m(2,1) = 1.1537*(imu.my+8.9233);
fbob 2:dc7840a67f77 106 m(3,1) = 0.9982*(imu.mz-11.8958);
fbob 2:dc7840a67f77 107 }
fbob 2:dc7840a67f77 108
fbob 2:dc7840a67f77 109 void AttitudeEstimator::get_output()
fbob 2:dc7840a67f77 110 {
fbob 2:dc7840a67f77 111 a = a/norm(a);
fbob 2:dc7840a67f77 112 m = m/norm(m);
fbob 2:dc7840a67f77 113
fbob 2:dc7840a67f77 114 Matrix t1(3,1), t2(3,1), t3(3,1);
fbob 2:dc7840a67f77 115 t1 = a;
fbob 2:dc7840a67f77 116 //t2 = cross(a,m)/norm(cross(a,m));
fbob 2:dc7840a67f77 117 t2 = cross(a,m);
fbob 2:dc7840a67f77 118 t2 = t2/norm(t2);
fbob 2:dc7840a67f77 119 t3 = cross(t1,t2);
fbob 2:dc7840a67f77 120
fbob 2:dc7840a67f77 121 /*Matrix BT(3,3), NT(3,3);
fbob 2:dc7840a67f77 122 BT(1,1) = t1(1,1);
fbob 2:dc7840a67f77 123 BT(2,1) = t1(2,1);
fbob 2:dc7840a67f77 124 BT(3,1) = t1(3,1);
fbob 2:dc7840a67f77 125 BT(1,2) = t2(1,1);
fbob 2:dc7840a67f77 126 BT(2,2) = t2(2,1);
fbob 2:dc7840a67f77 127 BT(3,2) = t2(3,1);
fbob 2:dc7840a67f77 128 BT(1,3) = t3(1,1);
fbob 2:dc7840a67f77 129 BT(2,3) = t3(2,1);
fbob 2:dc7840a67f77 130 BT(3,3) = t3(3,1);
fbob 2:dc7840a67f77 131
fbob 2:dc7840a67f77 132 NT(1,2) = -0.3666;
fbob 2:dc7840a67f77 133 NT(1,3) = -0.9304;
fbob 2:dc7840a67f77 134 NT(2,2) = -0.9304;
fbob 2:dc7840a67f77 135 NT(2,3) = 0.3666;
fbob 2:dc7840a67f77 136 NT(3,1) = -1.0;
fbob 2:dc7840a67f77 137
fbob 2:dc7840a67f77 138 BN = BT*transpose(NT);*/
fbob 1:085840a3d767 139
fbob 1:085840a3d767 140
fbob 2:dc7840a67f77 141 BN(1,1) = -t3(1,1);
fbob 2:dc7840a67f77 142 BN(2,1) = -t3(2,1);
fbob 2:dc7840a67f77 143 BN(3,1) = -t3(3,1);
fbob 2:dc7840a67f77 144 BN(1,2) = -t2(1,1);
fbob 2:dc7840a67f77 145 BN(2,2) = -t2(2,1);
fbob 2:dc7840a67f77 146 BN(3,2) = -t2(3,1);
fbob 2:dc7840a67f77 147 BN(1,3) = -t1(1,1);
fbob 2:dc7840a67f77 148 BN(2,3) = -t1(2,1);
fbob 2:dc7840a67f77 149 BN(3,3) = -t1(3,1);
fbob 2:dc7840a67f77 150
fbob 2:dc7840a67f77 151 /*q(4,1) = 0.0;
fbob 2:dc7840a67f77 152
fbob 2:dc7840a67f77 153 float tr = trace(BN);
fbob 2:dc7840a67f77 154 if (tr > 0.0) {
fbob 2:dc7840a67f77 155 float sqtrp1 = sqrt( tr + 1.0);
fbob 2:dc7840a67f77 156 q(1,1) = 0.5*sqtrp1;
fbob 2:dc7840a67f77 157 q(2,1) = (BN(2,3) - BN(3,2))/(2.0*sqtrp1);
fbob 2:dc7840a67f77 158 q(3,1) = (BN(3,1) - BN(1,3))/(2.0*sqtrp1);
fbob 2:dc7840a67f77 159 q(4,1) = (BN(1,2) - BN(2,1))/(2.0*sqtrp1);
fbob 2:dc7840a67f77 160 } else {
fbob 2:dc7840a67f77 161 if ((BN(2,2) > BN(1,1)) && (BN(2,2) > BN(3,3))) {
fbob 2:dc7840a67f77 162 float sqdip1 = sqrt(BN(2,2) - BN(1,1) - BN(3,3) + 1.0 );
fbob 2:dc7840a67f77 163 q(3,1) = 0.5*sqdip1;
fbob 2:dc7840a67f77 164 if ( sqdip1 != 0 ) {
fbob 2:dc7840a67f77 165 sqdip1 = 0.5/sqdip1;
fbob 2:dc7840a67f77 166 }
fbob 2:dc7840a67f77 167 q(1,1) = (BN(3,1) - BN(1,3))*sqdip1;
fbob 2:dc7840a67f77 168 q(2,1) = (BN(1,2) + BN(2,1))*sqdip1;
fbob 2:dc7840a67f77 169 q(4,1) = (BN(2,3) + BN(3,2))*sqdip1;
fbob 2:dc7840a67f77 170 } else if (BN(3,3) > BN(1,1)) {
fbob 2:dc7840a67f77 171 float sqdip1 = sqrt(BN(3,3) - BN(1,1) - BN(2,2) + 1.0 );
fbob 2:dc7840a67f77 172 q(4,1) = 0.5*sqdip1;
fbob 2:dc7840a67f77 173 if ( sqdip1 != 0 ) {
fbob 2:dc7840a67f77 174 sqdip1 = 0.5/sqdip1;
fbob 2:dc7840a67f77 175 }
fbob 2:dc7840a67f77 176 q(1,1) = (BN(1,2) - BN(2,1))*sqdip1;
fbob 2:dc7840a67f77 177 q(2,1) = (BN(3,1) + BN(1,3))*sqdip1;
fbob 2:dc7840a67f77 178 q(3,1) = (BN(2,3) + BN(3,2))*sqdip1;
fbob 2:dc7840a67f77 179 } else {
fbob 2:dc7840a67f77 180 float sqdip1 = sqrt(BN(1,1) - BN(2,2) - BN(3,3) + 1.0 );
fbob 2:dc7840a67f77 181 q(2,1) = 0.5*sqdip1;
fbob 2:dc7840a67f77 182 if ( sqdip1 != 0 ) {
fbob 2:dc7840a67f77 183 sqdip1 = 0.5/sqdip1;
fbob 2:dc7840a67f77 184 }
fbob 2:dc7840a67f77 185 q(1,1) = (BN(2,3) - BN(3,2))*sqdip1;
fbob 2:dc7840a67f77 186 q(3,1) = (BN(1,2) + BN(2,1))*sqdip1;
fbob 2:dc7840a67f77 187 q(4,1) = (BN(3,1) + BN(1,3))*sqdip1;
fbob 2:dc7840a67f77 188 }
fbob 2:dc7840a67f77 189 }*/
fbob 1:085840a3d767 190
fbob 2:dc7840a67f77 191 q = dcm2quat(BN);
fbob 2:dc7840a67f77 192
fbob 2:dc7840a67f77 193 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)))) {
fbob 2:dc7840a67f77 194 if (((x(1,1) > 0) && (q(1,1) < 0)) || ((x(1,1) < 0) && (q(1,1) > 0))) {
fbob 2:dc7840a67f77 195 q = -q;
fbob 2:dc7840a67f77 196 }
fbob 2:dc7840a67f77 197 } else if ((abs(x(2,1)) > abs(x(3,1))) && (abs(x(2,1)) > abs(x(4,1)))) {
fbob 2:dc7840a67f77 198 if (((x(2,1) > 0) && (q(2,1) < 0)) || ((x(2,1) < 0) && (q(2,1) > 0))) {
fbob 2:dc7840a67f77 199 q = -q;
fbob 2:dc7840a67f77 200 }
fbob 2:dc7840a67f77 201 } else if ((abs(x(3,1)) > abs(x(4,1)))) {
fbob 2:dc7840a67f77 202 if (((x(3,1) > 0) && (q(3,1) < 0)) || ((x(3,1) < 0) && (q(3,1) > 0))) {
fbob 2:dc7840a67f77 203 q = -q;
fbob 2:dc7840a67f77 204 }
fbob 2:dc7840a67f77 205 } else {
fbob 2:dc7840a67f77 206 if (((x(4,1) > 0) && (q(4,1) < 0)) || ((x(4,1) < 0) && (q(4,1) > 0))) {
fbob 2:dc7840a67f77 207 q = -q;
fbob 2:dc7840a67f77 208 }
fbob 2:dc7840a67f77 209 }
fbob 2:dc7840a67f77 210 }