Fabio Bobrow
/
Cubli
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Embed:
(wiki syntax)
Show/hide line numbers
AttitudeEstimator.cpp
00001 #include "mbed.h" 00002 #include "AttitudeEstimator.h" 00003 00004 // Class constructor 00005 AttitudeEstimator::AttitudeEstimator() : imu(A4,A5) 00006 { 00007 } 00008 00009 // Initialize class 00010 void AttitudeEstimator::init() 00011 { 00012 // Initialize IMU sensor object 00013 imu.init(); 00014 00015 dt = 0.005; 00016 dt_half = dt/2.0; 00017 00018 x = eye(4,1); 00019 A = eye(4); 00020 P = eye(4); 00021 Q = 0.001*eye(4); 00022 R = 10.0*eye(4); 00023 H = eye(4); 00024 I = eye(4); 00025 00026 g = zeros(3,1); 00027 a = zeros(3,1); 00028 m = zeros(3,1); 00029 00030 q = zeros(4,1); 00031 00032 BN = eye(3); 00033 00034 } 00035 00036 // Estimate euler angles (rad) and angular velocity (rad/s) 00037 void AttitudeEstimator::estimate() 00038 { 00039 // Read IMU sensor data 00040 read(); 00041 get_output(); 00042 00043 float omega_x = g(1,1)*dt_half; 00044 float omega_y = g(2,1)*dt_half; 00045 float omega_z = g(3,1)*dt_half; 00046 00047 /*A(1,1) = 1.0; 00048 A(1,2) = -g(1,1)*dt/2.0; 00049 A(1,3) = -g(2,1)*dt/2.0; 00050 A(1,4) = -g(3,1)*dt/2.0; 00051 A(2,1) = g(1,1)*dt/2.0; 00052 A(2,2) = 1.0; 00053 A(2,3) = g(3,1)*dt/2.0; 00054 A(2,4) = -g(2,1)*dt/2.0; 00055 A(3,1) = g(2,1)*dt/2.0; 00056 A(3,2) = -g(3,1)*dt/2.0; 00057 A(3,3) = 1.0; 00058 A(3,4) = g(1,1)*dt/2.0; 00059 A(4,1) = g(3,1)*dt/2.0; 00060 A(4,2) = g(2,1)*dt/2.0; 00061 A(4,3) = -g(1,1)*dt/2.0; 00062 A(4,4) = 1.0;*/ 00063 00064 A(1,2) = -omega_x; 00065 A(1,3) = -omega_y; 00066 A(1,4) = -omega_z; 00067 A(2,1) = omega_x; 00068 A(2,3) = omega_z; 00069 A(2,4) = -omega_y; 00070 A(3,1) = omega_y; 00071 A(3,2) = -omega_z; 00072 A(3,4) = omega_x; 00073 A(4,1) = omega_z; 00074 A(4,2) = omega_y; 00075 A(4,3) = -omega_x; 00076 00077 x = A*x; 00078 x = x/norm(x); 00079 00080 P = A*P*transpose(A)+Q; 00081 00082 //K = P*transpose(H)*inverse(H*P*transpose(H)+R); 00083 K = P*inverse(P+R); 00084 00085 //x = x+K*(q-H*x); 00086 x = x+K*(q-x); 00087 x = x/norm(x); 00088 //P = P-K*H*P; 00089 P = P-K*P; 00090 //P = P*(I-K*H); 00091 //P = P-K*(H*P*transpose(H)+R)*transpose(K); 00092 //P = (eye(4)-K*H)*P*transpose(eye(4)-K*H)+K*R*transpose(K); 00093 } 00094 00095 void AttitudeEstimator::read() 00096 { 00097 imu.read(); 00098 g(1,1) = imu.gx-0.0099; 00099 g(2,1) = imu.gy+0.0693; 00100 g(3,1) = imu.gz-0.0339; 00101 a(1,1) = 1.0077*(imu.ax-0.0975); 00102 a(2,1) = 1.0061*(imu.ay-0.0600); 00103 a(3,1) = 0.9926*(imu.az-0.5156); 00104 m(1,1) = 0.8838*(imu.mx+21.0631); 00105 m(2,1) = 1.1537*(imu.my+8.9233); 00106 m(3,1) = 0.9982*(imu.mz-11.8958); 00107 } 00108 00109 void AttitudeEstimator::get_output() 00110 { 00111 a = a/norm(a); 00112 m = m/norm(m); 00113 00114 Matrix t1(3,1), t2(3,1), t3(3,1); 00115 t1 = a; 00116 //t2 = cross(a,m)/norm(cross(a,m)); 00117 t2 = cross(a,m); 00118 t2 = t2/norm(t2); 00119 t3 = cross(t1,t2); 00120 00121 /*Matrix BT(3,3), NT(3,3); 00122 BT(1,1) = t1(1,1); 00123 BT(2,1) = t1(2,1); 00124 BT(3,1) = t1(3,1); 00125 BT(1,2) = t2(1,1); 00126 BT(2,2) = t2(2,1); 00127 BT(3,2) = t2(3,1); 00128 BT(1,3) = t3(1,1); 00129 BT(2,3) = t3(2,1); 00130 BT(3,3) = t3(3,1); 00131 00132 NT(1,2) = -0.3666; 00133 NT(1,3) = -0.9304; 00134 NT(2,2) = -0.9304; 00135 NT(2,3) = 0.3666; 00136 NT(3,1) = -1.0; 00137 00138 BN = BT*transpose(NT);*/ 00139 00140 00141 BN(1,1) = -t3(1,1); 00142 BN(2,1) = -t3(2,1); 00143 BN(3,1) = -t3(3,1); 00144 BN(1,2) = -t2(1,1); 00145 BN(2,2) = -t2(2,1); 00146 BN(3,2) = -t2(3,1); 00147 BN(1,3) = -t1(1,1); 00148 BN(2,3) = -t1(2,1); 00149 BN(3,3) = -t1(3,1); 00150 00151 /*q(4,1) = 0.0; 00152 00153 float tr = trace(BN); 00154 if (tr > 0.0) { 00155 float sqtrp1 = sqrt( tr + 1.0); 00156 q(1,1) = 0.5*sqtrp1; 00157 q(2,1) = (BN(2,3) - BN(3,2))/(2.0*sqtrp1); 00158 q(3,1) = (BN(3,1) - BN(1,3))/(2.0*sqtrp1); 00159 q(4,1) = (BN(1,2) - BN(2,1))/(2.0*sqtrp1); 00160 } else { 00161 if ((BN(2,2) > BN(1,1)) && (BN(2,2) > BN(3,3))) { 00162 float sqdip1 = sqrt(BN(2,2) - BN(1,1) - BN(3,3) + 1.0 ); 00163 q(3,1) = 0.5*sqdip1; 00164 if ( sqdip1 != 0 ) { 00165 sqdip1 = 0.5/sqdip1; 00166 } 00167 q(1,1) = (BN(3,1) - BN(1,3))*sqdip1; 00168 q(2,1) = (BN(1,2) + BN(2,1))*sqdip1; 00169 q(4,1) = (BN(2,3) + BN(3,2))*sqdip1; 00170 } else if (BN(3,3) > BN(1,1)) { 00171 float sqdip1 = sqrt(BN(3,3) - BN(1,1) - BN(2,2) + 1.0 ); 00172 q(4,1) = 0.5*sqdip1; 00173 if ( sqdip1 != 0 ) { 00174 sqdip1 = 0.5/sqdip1; 00175 } 00176 q(1,1) = (BN(1,2) - BN(2,1))*sqdip1; 00177 q(2,1) = (BN(3,1) + BN(1,3))*sqdip1; 00178 q(3,1) = (BN(2,3) + BN(3,2))*sqdip1; 00179 } else { 00180 float sqdip1 = sqrt(BN(1,1) - BN(2,2) - BN(3,3) + 1.0 ); 00181 q(2,1) = 0.5*sqdip1; 00182 if ( sqdip1 != 0 ) { 00183 sqdip1 = 0.5/sqdip1; 00184 } 00185 q(1,1) = (BN(2,3) - BN(3,2))*sqdip1; 00186 q(3,1) = (BN(1,2) + BN(2,1))*sqdip1; 00187 q(4,1) = (BN(3,1) + BN(1,3))*sqdip1; 00188 } 00189 }*/ 00190 00191 q = dcm2quat(BN); 00192 00193 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)))) { 00194 if (((x(1,1) > 0) && (q(1,1) < 0)) || ((x(1,1) < 0) && (q(1,1) > 0))) { 00195 q = -q; 00196 } 00197 } else if ((abs(x(2,1)) > abs(x(3,1))) && (abs(x(2,1)) > abs(x(4,1)))) { 00198 if (((x(2,1) > 0) && (q(2,1) < 0)) || ((x(2,1) < 0) && (q(2,1) > 0))) { 00199 q = -q; 00200 } 00201 } else if ((abs(x(3,1)) > abs(x(4,1)))) { 00202 if (((x(3,1) > 0) && (q(3,1) < 0)) || ((x(3,1) < 0) && (q(3,1) > 0))) { 00203 q = -q; 00204 } 00205 } else { 00206 if (((x(4,1) > 0) && (q(4,1) < 0)) || ((x(4,1) < 0) && (q(4,1) > 0))) { 00207 q = -q; 00208 } 00209 } 00210 }
Generated on Mon Jul 18 2022 10:36:03 by
1.7.2