HAPSRG / solaESKF_Eigen

Eigen

Dependencies:   Eigen

Dependents:   optWingforHAPS_Eigen hexaTest_Eigen

solaESKF.cpp@73:5770a0d470c0, 2021-11-21 (annotated)

Committer:
NaotoMorita
Date:
Sun Nov 21 08:24:40 2021 +0000
Revision:
73:5770a0d470c0
Parent:
72:8ebae608ae12
Child:
74:f5fe7fecbd3c
heading fuse

Who changed what in which revision?

UserRevisionLine numberNew contents of line
NaotoMorita 44:7d82e63b6a86 1 #include "solaESKF.hpp"
NaotoMorita 54:cd514d9d4b19 2
NaotoMorita 19:3fae66745363 3
NaotoMorita 19:3fae66745363 4
NaotoMorita 44:7d82e63b6a86 5 solaESKF::solaESKF()
NaotoMorita 70:d12e46fdc2f0 6 :pihat(3,1),vihat(3,1),qhat(4,1),accBias(3,1),gyroBias(3,1),gravity(3,1),magBias(3,1),errState(16,1),Phat(16,16),Q(16,16)
NaotoMorita 62:5519d34eb6e8 7 {
NaotoMorita 46:15988dc41923 8 pihat << 0.0f << 0.0f << 0.0f;
NaotoMorita 46:15988dc41923 9 vihat << 0.0f << 0.0f << 0.0f;
NaotoMorita 46:15988dc41923 10 qhat << 1.0f << 0.0f << 0.0f << 0.0f;
NaotoMorita 46:15988dc41923 11 accBias << 0.0f << 0.0f << 0.0f;
NaotoMorita 46:15988dc41923 12 gyroBias << 0.0f << 0.0f << 0.0f;
NaotoMorita 46:15988dc41923 13 gravity << 0.0f << 0.0f << 9.8f;
NaotoMorita 62:5519d34eb6e8 14 magBias << 0.0f << 0.0f << 0.0f;
NaotoMorita 62:5519d34eb6e8 15 magRadius = 0.0f;
NaotoMorita 58:93ba28cf5cb3 16
NaotoMorita 62:5519d34eb6e8 17 nState = errState.getRows();
NaotoMorita 62:5519d34eb6e8 18
NaotoMorita 58:93ba28cf5cb3 19 for (int i = 1; i < nState+1; i++){
NaotoMorita 46:15988dc41923 20 errState(i,1) = 0.0f;
NaotoMorita 58:93ba28cf5cb3 21 for (int j = 1; j < nState+1; j++){
NaotoMorita 46:15988dc41923 22 Phat(i,j) = 0.0f;
NaotoMorita 46:15988dc41923 23 Q(i,j) = 0.0f;
NaotoMorita 46:15988dc41923 24 }
NaotoMorita 46:15988dc41923 25 }
NaotoMorita 62:5519d34eb6e8 26
NaotoMorita 46:15988dc41923 27 setBlockDiag(Phat,0.1f,1,3);//position
NaotoMorita 46:15988dc41923 28 setBlockDiag(Phat,0.1f,4,6);//velocity
NaotoMorita 46:15988dc41923 29 setBlockDiag(Phat,0.1f,7,9);//angle error
NaotoMorita 47:2467de40951f 30 setBlockDiag(Phat,0.1f,10,12);//acc bias
NaotoMorita 47:2467de40951f 31 setBlockDiag(Phat,0.1f,13,15);//gyro bias
NaotoMorita 62:5519d34eb6e8 32 setBlockDiag(Phat,0.00000001f,16,16);//gravity
NaotoMorita 47:2467de40951f 33 setBlockDiag(Q,0.00025f,4,6);//velocity
NaotoMorita 55:21611d4cf7e8 34 setBlockDiag(Q,0.005f/57.0f,7,9);//angle error
NaotoMorita 46:15988dc41923 35 setBlockDiag(Q,0.001f,10,12);//acc bias
NaotoMorita 70:d12e46fdc2f0 36 setBlockDiag(Q,0.001f,13,15);//gyro bias//positionとgravityはQ項なし
NaotoMorita 19:3fae66745363 37
NaotoMorita 19:3fae66745363 38 }
NaotoMorita 19:3fae66745363 39
NaotoMorita 47:2467de40951f 40
NaotoMorita 44:7d82e63b6a86 41 void solaESKF::updateNominal(Matrix acc, Matrix gyro,float att_dt)
NaotoMorita 19:3fae66745363 42 {
NaotoMorita 44:7d82e63b6a86 43 Matrix gyrom = gyro - gyroBias;
NaotoMorita 44:7d82e63b6a86 44 Matrix accm = acc - accBias;
NaotoMorita 44:7d82e63b6a86 45
NaotoMorita 44:7d82e63b6a86 46 Matrix qint(4,1);
NaotoMorita 46:15988dc41923 47 qint << 1.0f << 0.5f*gyrom(1,1)*att_dt << 0.5f*gyrom(2,1)*att_dt << 0.5f*gyrom(3,1)*att_dt;
NaotoMorita 44:7d82e63b6a86 48 qhat = quatmultiply(qhat,qint);
NaotoMorita 19:3fae66745363 49 float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qhat),qhat));
NaotoMorita 19:3fae66745363 50 qhat *= (1.0f/ qnorm);
NaotoMorita 23:1509648c2318 51
NaotoMorita 23:1509648c2318 52 Matrix dcm(3,3);
NaotoMorita 23:1509648c2318 53 computeDcm(dcm, qhat);
NaotoMorita 46:15988dc41923 54 Matrix accned = dcm*accm+gravity;
NaotoMorita 44:7d82e63b6a86 55 vihat += accned*att_dt;
NaotoMorita 44:7d82e63b6a86 56
NaotoMorita 44:7d82e63b6a86 57 pihat += vihat*att_dt+0.5f*accned*att_dt*att_dt;
NaotoMorita 19:3fae66745363 58 }
NaotoMorita 19:3fae66745363 59
NaotoMorita 44:7d82e63b6a86 60 void solaESKF::updateErrState(Matrix acc,Matrix gyro,float att_dt)
NaotoMorita 23:1509648c2318 61 {
NaotoMorita 44:7d82e63b6a86 62 Matrix gyrom = gyro - gyroBias;
NaotoMorita 44:7d82e63b6a86 63 Matrix accm = acc - accBias;
NaotoMorita 23:1509648c2318 64
NaotoMorita 23:1509648c2318 65 Matrix dcm(3,3);
NaotoMorita 23:1509648c2318 66 computeDcm(dcm, qhat);
NaotoMorita 47:2467de40951f 67 Matrix a2v = -dcm*MatrixMath::Matrixcross(accm(1,1),accm(2,1),accm(3,1));
NaotoMorita 44:7d82e63b6a86 68
NaotoMorita 47:2467de40951f 69 Matrix A(nState,nState);
NaotoMorita 44:7d82e63b6a86 70 //position
NaotoMorita 47:2467de40951f 71 A(1,4) = 1.0f;
NaotoMorita 47:2467de40951f 72 A(2,5) = 1.0f;
NaotoMorita 47:2467de40951f 73 A(3,6) = 1.0f;
NaotoMorita 47:2467de40951f 74
NaotoMorita 47:2467de40951f 75 //velocity
NaotoMorita 46:15988dc41923 76 for (int i = 1; i < 4; i++){
NaotoMorita 46:15988dc41923 77 for (int j = 1; j < 4; j++){
NaotoMorita 47:2467de40951f 78 A(i+3,j+6) = a2v(i,j);
NaotoMorita 47:2467de40951f 79 A(i+3,j+9) = -dcm(i,j);
NaotoMorita 47:2467de40951f 80
NaotoMorita 23:1509648c2318 81 }
NaotoMorita 23:1509648c2318 82 }
NaotoMorita 59:03fe5e16a33c 83 A(6,16) = 1.0f;
NaotoMorita 23:1509648c2318 84
NaotoMorita 44:7d82e63b6a86 85 //angulat error
NaotoMorita 47:2467de40951f 86 A(7,8) = gyrom(3,1);
NaotoMorita 47:2467de40951f 87 A(7,9) = -gyrom(2,1);
NaotoMorita 47:2467de40951f 88 A(8,7) = -gyrom(3,1);
NaotoMorita 47:2467de40951f 89 A(8,9) = gyrom(1,1);
NaotoMorita 47:2467de40951f 90 A(9,7) = gyrom(2,1);
NaotoMorita 47:2467de40951f 91 A(9,8) = -gyrom(1,1);
NaotoMorita 47:2467de40951f 92 A(7,13) = -1.0f;
NaotoMorita 47:2467de40951f 93 A(8,14) = -1.0f;
NaotoMorita 47:2467de40951f 94 A(9,15) = -1.0f;
NaotoMorita 25:07ac5c6cd61c 95
NaotoMorita 47:2467de40951f 96
NaotoMorita 47:2467de40951f 97 Matrix Fx = MatrixMath::Eye(nState) + A * att_dt;// + (0.5f*att_dt*att_dt) * A * A;
NaotoMorita 59:03fe5e16a33c 98 for (int i = 1; i < nState+1; i++){
NaotoMorita 59:03fe5e16a33c 99 if(i>3 && i<10){
NaotoMorita 59:03fe5e16a33c 100 Q(i,i) *=att_dt;
NaotoMorita 59:03fe5e16a33c 101 }else if(i>9 && i<16){
NaotoMorita 59:03fe5e16a33c 102 Q(i,i) *= att_dt*att_dt;
NaotoMorita 59:03fe5e16a33c 103 }else if(i>16 && i<nState+1){
NaotoMorita 59:03fe5e16a33c 104 Q(i,i) *=att_dt*att_dt;
NaotoMorita 59:03fe5e16a33c 105 }else{
NaotoMorita 59:03fe5e16a33c 106 Q(i,i) = 0.0f;
NaotoMorita 59:03fe5e16a33c 107 }
NaotoMorita 59:03fe5e16a33c 108 }
NaotoMorita 44:7d82e63b6a86 109 errState = Fx * errState;
NaotoMorita 59:03fe5e16a33c 110 Phat = Fx*Phat*MatrixMath::Transpose(Fx)+Q;
NaotoMorita 25:07ac5c6cd61c 111 }
NaotoMorita 25:07ac5c6cd61c 112
NaotoMorita 61:5e5c4fe12440 113 void solaESKF::updateAcc(Matrix acc,Matrix R)
NaotoMorita 46:15988dc41923 114 {
NaotoMorita 46:15988dc41923 115 Matrix accm = acc - accBias;
NaotoMorita 56:c10f1168bd4a 116 Matrix dcm(3,3);
NaotoMorita 56:c10f1168bd4a 117 computeDcm(dcm, qhat);
NaotoMorita 56:c10f1168bd4a 118 Matrix tdcm = MatrixMath::Transpose(dcm);
NaotoMorita 46:15988dc41923 119 Matrix tdcm_g = tdcm*gravity;
NaotoMorita 59:03fe5e16a33c 120 Matrix rotgrav = MatrixMath::Matrixcross(tdcm_g(1,1),tdcm_g(2,1),tdcm_g(3,1));
NaotoMorita 46:15988dc41923 121
NaotoMorita 61:5e5c4fe12440 122 Matrix H(3,nState);
NaotoMorita 59:03fe5e16a33c 123 for (int i = 1; i < 4; i++){
NaotoMorita 59:03fe5e16a33c 124 for (int j = 1; j < 4; j++){
NaotoMorita 59:03fe5e16a33c 125 H(i,j+6) = rotgrav(i,j);
NaotoMorita 46:15988dc41923 126 }
NaotoMorita 59:03fe5e16a33c 127 H(i,16) = tdcm(i,3);
NaotoMorita 58:93ba28cf5cb3 128 }
NaotoMorita 56:c10f1168bd4a 129
NaotoMorita 46:15988dc41923 130 H(1,10) = -1.0f;
NaotoMorita 46:15988dc41923 131 H(2,11) = -1.0f;
NaotoMorita 46:15988dc41923 132 H(3,12) = -1.0f;
NaotoMorita 59:03fe5e16a33c 133
NaotoMorita 61:5e5c4fe12440 134 Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
NaotoMorita 61:5e5c4fe12440 135 Matrix zacc = -accm-tdcm*gravity;
NaotoMorita 61:5e5c4fe12440 136 Matrix z(3,1);
NaotoMorita 61:5e5c4fe12440 137 z << zacc(1,1) << zacc(2,1) << zacc(3,1);
NaotoMorita 61:5e5c4fe12440 138 errState = K * z;
NaotoMorita 61:5e5c4fe12440 139 Phat = (MatrixMath::Eye(nState)-K*H)*Phat;
NaotoMorita 61:5e5c4fe12440 140
NaotoMorita 61:5e5c4fe12440 141 fuseErr2Nominal();
NaotoMorita 61:5e5c4fe12440 142 }
NaotoMorita 61:5e5c4fe12440 143 void solaESKF::updateMag(Matrix mag,Matrix R)
NaotoMorita 61:5e5c4fe12440 144 {
NaotoMorita 62:5519d34eb6e8 145 Matrix magm = mag-magBias;
NaotoMorita 61:5e5c4fe12440 146 Matrix dcm(3,3);
NaotoMorita 61:5e5c4fe12440 147 computeDcm(dcm, qhat);
NaotoMorita 61:5e5c4fe12440 148 Matrix tdcm = MatrixMath::Transpose(dcm);
NaotoMorita 61:5e5c4fe12440 149 Matrix rotmag = -dcm*MatrixMath::Matrixcross(magm(1,1),magm(2,1),magm(3,1));
NaotoMorita 61:5e5c4fe12440 150
NaotoMorita 68:264a7e0e4a29 151 Matrix H(2,nState);
NaotoMorita 63:74a9565a0141 152 int nH = H.getRows();
NaotoMorita 61:5e5c4fe12440 153
NaotoMorita 59:03fe5e16a33c 154 Matrix magned = dcm*magm;
NaotoMorita 59:03fe5e16a33c 155 float hx = sqrt(magned(1,1)*magned(1,1)+magned(2,1)*magned(2,1));
NaotoMorita 63:74a9565a0141 156 float hz = sqrt(magned(3,1)*magned(3,1));
NaotoMorita 62:5519d34eb6e8 157 float a = sqrt(magm(1,1)*magm(1,1)+magm(2,1)*magm(2,1)+magm(3,1)*magm(3,1));
NaotoMorita 61:5e5c4fe12440 158
NaotoMorita 68:264a7e0e4a29 159 H(1,3+6) = rotmag(1,3)-(rotmag(1,3)+rotmag(2,3))/hx;
NaotoMorita 68:264a7e0e4a29 160 H(2,3+6) = rotmag(2,3);
NaotoMorita 59:03fe5e16a33c 161 for(int j = 1; j < 4; j++){
NaotoMorita 65:c25d7810de44 162 //H(3,j+6) = rotmag(3,j)-(rotmag(3,j))/hz;
NaotoMorita 65:c25d7810de44 163 H(1,j+16) = -dcm(1,j)+(dcm(1,j)+dcm(2,j))/hx;
NaotoMorita 62:5519d34eb6e8 164 H(2,j+16) = -dcm(2,j);
NaotoMorita 65:c25d7810de44 165 //H(3,j+16) = -dcm(3,j)+(dcm(3,j))/hz;
NaotoMorita 63:74a9565a0141 166 }
NaotoMorita 68:264a7e0e4a29 167 //H(3,20) = 1.0f;
NaotoMorita 68:264a7e0e4a29 168 //H(3,17) = magm(1,1)/a;
NaotoMorita 68:264a7e0e4a29 169 //H(3,18) = magm(2,1)/a;
NaotoMorita 68:264a7e0e4a29 170 //H(3,19) = magm(3,1)/a;
NaotoMorita 63:74a9565a0141 171
NaotoMorita 63:74a9565a0141 172 Matrix z(nH,1);
NaotoMorita 68:264a7e0e4a29 173 z << -(magned(1,1) - hx) << -magned(2,1);
NaotoMorita 63:74a9565a0141 174 //twelite.printf("%f %f %f %f\r\n",-(magned(1,1) - hx),-magned(2,1),-(magned(3,1) - hz),-(magRadius-a));
NaotoMorita 65:c25d7810de44 175 Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
NaotoMorita 63:74a9565a0141 176
NaotoMorita 65:c25d7810de44 177 /*
NaotoMorita 63:74a9565a0141 178 Matrix r3(3,1);
NaotoMorita 63:74a9565a0141 179 r3 << tdcm(1,3)<< tdcm(2,3) << tdcm(3,3);
NaotoMorita 63:74a9565a0141 180 Matrix kpart = r3*MatrixMath::Transpose(r3);
NaotoMorita 63:74a9565a0141 181 Matrix Pm(nState,nState);
NaotoMorita 63:74a9565a0141 182 for(int i = 7; i<10; i++){
NaotoMorita 63:74a9565a0141 183 for(int j = 7;j<10;j++){
NaotoMorita 63:74a9565a0141 184 Pm(i,j) = Phat(i,j);
NaotoMorita 63:74a9565a0141 185 }
NaotoMorita 59:03fe5e16a33c 186 }
NaotoMorita 63:74a9565a0141 187 for(int i = 17; i<nState+1; i++){
NaotoMorita 63:74a9565a0141 188 for(int j = 17;j<nState+1;j++){
NaotoMorita 63:74a9565a0141 189 Pm(i,j) = Phat(i,j);
NaotoMorita 63:74a9565a0141 190 }
NaotoMorita 63:74a9565a0141 191 }
NaotoMorita 61:5e5c4fe12440 192
NaotoMorita 63:74a9565a0141 193 Matrix K = (Pm*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Pm*MatrixMath::Transpose(H)+R);
NaotoMorita 63:74a9565a0141 194 Matrix Kmod(3,nH);
NaotoMorita 63:74a9565a0141 195 for(int i = 1; i<4; i++){
NaotoMorita 63:74a9565a0141 196 for(int j = 1;j<nH+1;j++){
NaotoMorita 63:74a9565a0141 197 Kmod(i,j) = K(i+6,j);
NaotoMorita 63:74a9565a0141 198 }
NaotoMorita 63:74a9565a0141 199 }
NaotoMorita 63:74a9565a0141 200 Kmod = kpart*Kmod;
NaotoMorita 63:74a9565a0141 201 for(int i = 1; i<nState+1; i++){
NaotoMorita 63:74a9565a0141 202 for(int j = 1;j<nH;j++){
NaotoMorita 63:74a9565a0141 203 if(i>6 && i<10){
NaotoMorita 63:74a9565a0141 204 K(i,j) = Kmod(i-6,j);
NaotoMorita 63:74a9565a0141 205 }else if(i<17){
NaotoMorita 63:74a9565a0141 206 K(i,j) = 0.0f;
NaotoMorita 63:74a9565a0141 207 }
NaotoMorita 63:74a9565a0141 208 }
NaotoMorita 63:74a9565a0141 209 }
NaotoMorita 65:c25d7810de44 210 */
NaotoMorita 65:c25d7810de44 211 errState = K * z;
NaotoMorita 65:c25d7810de44 212 Phat = (MatrixMath::Eye(nState)-K*H)*Phat;
NaotoMorita 63:74a9565a0141 213
NaotoMorita 65:c25d7810de44 214 fuseErr2Nominal();
NaotoMorita 65:c25d7810de44 215 }
NaotoMorita 70:d12e46fdc2f0 216
NaotoMorita 73:5770a0d470c0 217 void solaESKF::updateHeading(float a,Matrix R)
NaotoMorita 70:d12e46fdc2f0 218 {
NaotoMorita 70:d12e46fdc2f0 219 float q0 = qhat(1,1);
NaotoMorita 70:d12e46fdc2f0 220 float q1 = qhat(2,1);
NaotoMorita 70:d12e46fdc2f0 221 float q2 = qhat(3,1);
NaotoMorita 70:d12e46fdc2f0 222 float q3 = qhat(4,1);
NaotoMorita 73:5770a0d470c0 223
NaotoMorita 73:5770a0d470c0 224 bool canUseA = false;
NaotoMorita 73:5770a0d470c0 225 const float SA0 = 2*q3;
NaotoMorita 73:5770a0d470c0 226 const float SA1 = 2*q2;
NaotoMorita 73:5770a0d470c0 227 const float SA2 = SA0*q0 + SA1*q1;
NaotoMorita 73:5770a0d470c0 228 const float SA3 = q0*q0 + q1*q1 - q2*q2 - q3*q3;
NaotoMorita 73:5770a0d470c0 229 float SA4, SA5_inv;
NaotoMorita 73:5770a0d470c0 230 if ((SA3*SA3) > 1e-6f) {
NaotoMorita 73:5770a0d470c0 231 SA4 = 1.0F/(SA3*SA3);
NaotoMorita 73:5770a0d470c0 232 SA5_inv = SA2*SA2*SA4 + 1;
NaotoMorita 73:5770a0d470c0 233 canUseA = fabsf(SA5_inv) > 1e-6f;
NaotoMorita 73:5770a0d470c0 234 }
NaotoMorita 73:5770a0d470c0 235
NaotoMorita 73:5770a0d470c0 236 bool canUseB = false;
NaotoMorita 73:5770a0d470c0 237 const float SB0 = 2*q0;
NaotoMorita 73:5770a0d470c0 238 const float SB1 = 2*q1;
NaotoMorita 73:5770a0d470c0 239 const float SB2 = SB0*q3 + SB1*q2;
NaotoMorita 73:5770a0d470c0 240 const float SB4 = q0*q0 + q1*q1 - q2*q2 - q3*q3;
NaotoMorita 73:5770a0d470c0 241 float SB3, SB5_inv;
NaotoMorita 73:5770a0d470c0 242 if ((SB2*SB2) > 1e-6f) {
NaotoMorita 73:5770a0d470c0 243 SB3 = 1.0F/(SB2*SB2);
NaotoMorita 73:5770a0d470c0 244 SB5_inv = SB3*SB4*SB4 + 1;
NaotoMorita 73:5770a0d470c0 245 canUseB = fabsf(SB5_inv) > 1e-6f;
NaotoMorita 73:5770a0d470c0 246 }
NaotoMorita 73:5770a0d470c0 247
NaotoMorita 73:5770a0d470c0 248 Matrix Hh(1,4);
NaotoMorita 73:5770a0d470c0 249
NaotoMorita 73:5770a0d470c0 250 if (canUseA && (!canUseB || fabsf(SA5_inv) >= fabsf(SB5_inv))) {
NaotoMorita 73:5770a0d470c0 251 const float SA5 = 1.0F/SA5_inv;
NaotoMorita 73:5770a0d470c0 252 const float SA6 = 1.0F/SA3;
NaotoMorita 73:5770a0d470c0 253 const float SA7 = SA2*SA4;
NaotoMorita 73:5770a0d470c0 254 const float SA8 = 2*SA7;
NaotoMorita 73:5770a0d470c0 255 const float SA9 = 2*SA6;
NaotoMorita 73:5770a0d470c0 256
NaotoMorita 73:5770a0d470c0 257 Hh(1,1) = SA5*(SA0*SA6 - SA8*q0);
NaotoMorita 73:5770a0d470c0 258 Hh(1,2) = SA5*(SA1*SA6 - SA8*q1);
NaotoMorita 73:5770a0d470c0 259 Hh(1,3) = SA5*(SA1*SA7 + SA9*q1);
NaotoMorita 73:5770a0d470c0 260 Hh(1,4) = SA5*(SA0*SA7 + SA9*q0);
NaotoMorita 73:5770a0d470c0 261 } else if (canUseB && (!canUseA || fabsf(SB5_inv) > fabsf(SA5_inv))) {
NaotoMorita 73:5770a0d470c0 262 const float SB5 = 1.0F/SB5_inv;
NaotoMorita 73:5770a0d470c0 263 const float SB6 = 1.0F/SB2;
NaotoMorita 73:5770a0d470c0 264 const float SB7 = SB3*SB4;
NaotoMorita 73:5770a0d470c0 265 const float SB8 = 2*SB7;
NaotoMorita 73:5770a0d470c0 266 const float SB9 = 2*SB6;
NaotoMorita 73:5770a0d470c0 267
NaotoMorita 73:5770a0d470c0 268 Hh(1,1) = -SB5*(SB0*SB6 - SB8*q3);
NaotoMorita 73:5770a0d470c0 269 Hh(1,2) = -SB5*(SB1*SB6 - SB8*q2);
NaotoMorita 73:5770a0d470c0 270 Hh(1,3) = -SB5*(-SB1*SB7 - SB9*q2);
NaotoMorita 73:5770a0d470c0 271 Hh(1,4) = -SB5*(-SB0*SB7 - SB9*q3);
NaotoMorita 73:5770a0d470c0 272 } else {
NaotoMorita 73:5770a0d470c0 273 return;
NaotoMorita 73:5770a0d470c0 274 }
NaotoMorita 70:d12e46fdc2f0 275
NaotoMorita 70:d12e46fdc2f0 276 Matrix Hdq(4,3);
NaotoMorita 70:d12e46fdc2f0 277 Hdq << -0.5f*q1 << -0.5f*q2 << -0.5f*q3
NaotoMorita 72:8ebae608ae12 278 << 0.5f*q0 << -0.5f*q3 << 0.5f*q2
NaotoMorita 72:8ebae608ae12 279 << 0.5f*q3 << 0.5f*q0 << -0.5f*q1
NaotoMorita 73:5770a0d470c0 280 << -0.5f*q2 << 0.5f*q1 << 0.5f*q0;
NaotoMorita 70:d12e46fdc2f0 281
NaotoMorita 70:d12e46fdc2f0 282 Matrix Hpart = Hh*Hdq;
NaotoMorita 73:5770a0d470c0 283 Matrix H(1,nState);
NaotoMorita 70:d12e46fdc2f0 284 for(int j=1;j<4;j++){
NaotoMorita 70:d12e46fdc2f0 285 H(1,j+6) = Hpart(1,j);
NaotoMorita 70:d12e46fdc2f0 286 }
NaotoMorita 70:d12e46fdc2f0 287
NaotoMorita 73:5770a0d470c0 288 float psi = atan2f(qhat(2,1)*qhat(3,1) + qhat(1,1)*qhat(4,1), 0.5f - qhat(3,1)*qhat(3,1) - qhat(4,1)*qhat(4,1));
NaotoMorita 73:5770a0d470c0 289 Matrix z(1,1);
NaotoMorita 73:5770a0d470c0 290 z << atan2(sin(a-psi),cos(a-psi));
NaotoMorita 70:d12e46fdc2f0 291 Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
NaotoMorita 70:d12e46fdc2f0 292 errState = K * z;
NaotoMorita 70:d12e46fdc2f0 293 Phat = (MatrixMath::Eye(nState)-K*H)*Phat;
NaotoMorita 70:d12e46fdc2f0 294
NaotoMorita 70:d12e46fdc2f0 295 fuseErr2Nominal();
NaotoMorita 70:d12e46fdc2f0 296 }
NaotoMorita 65:c25d7810de44 297 void solaESKF::updateImuConstraints(Matrix acc,Matrix mag,Matrix R)
NaotoMorita 65:c25d7810de44 298 {
NaotoMorita 65:c25d7810de44 299 Matrix accm = acc - accBias;
NaotoMorita 65:c25d7810de44 300 Matrix dcm(3,3);
NaotoMorita 65:c25d7810de44 301 computeDcm(dcm, qhat);
NaotoMorita 65:c25d7810de44 302 Matrix tdcm = MatrixMath::Transpose(dcm);
NaotoMorita 65:c25d7810de44 303 Matrix tdcm_g = tdcm*gravity;
NaotoMorita 65:c25d7810de44 304 Matrix rotgrav = MatrixMath::Matrixcross(tdcm_g(1,1),tdcm_g(2,1),tdcm_g(3,1));
NaotoMorita 70:d12e46fdc2f0 305 Matrix rotmag = -dcm*MatrixMath::Matrixcross(mag(1,1),mag(2,1),mag(3,1));
NaotoMorita 65:c25d7810de44 306
NaotoMorita 70:d12e46fdc2f0 307 Matrix H(6,nState);
NaotoMorita 70:d12e46fdc2f0 308 for (int i = 1; i < 3; i++){
NaotoMorita 65:c25d7810de44 309 for (int j = 1; j < 4; j++){
NaotoMorita 65:c25d7810de44 310 H(i,j+6) = rotgrav(i,j);
NaotoMorita 65:c25d7810de44 311 }
NaotoMorita 65:c25d7810de44 312 H(i,16) = tdcm(i,3);
NaotoMorita 65:c25d7810de44 313 }
NaotoMorita 65:c25d7810de44 314
NaotoMorita 65:c25d7810de44 315 H(1,10) = -1.0f;
NaotoMorita 65:c25d7810de44 316 H(2,11) = -1.0f;
NaotoMorita 65:c25d7810de44 317 H(3,12) = -1.0f;
NaotoMorita 65:c25d7810de44 318
NaotoMorita 70:d12e46fdc2f0 319 Matrix magned = dcm*mag;
NaotoMorita 65:c25d7810de44 320 float hx = sqrt(magned(1,1)*magned(1,1)+magned(2,1)*magned(2,1));
NaotoMorita 70:d12e46fdc2f0 321 float hz = sqrt(magned(3,1)*magned(3,1));
NaotoMorita 65:c25d7810de44 322
NaotoMorita 66:6a093cb91728 323 for(int j = 3; j < 4; j++){
NaotoMorita 65:c25d7810de44 324 H(4,j+6) = rotmag(1,j)-(rotmag(1,j)+rotmag(2,j))/hx;
NaotoMorita 66:6a093cb91728 325 //H(4,j+16) = -dcm(1,j)+(dcm(1,j)+dcm(2,j))/hx;
NaotoMorita 65:c25d7810de44 326 H(5,j+6) = rotmag(2,j);
NaotoMorita 66:6a093cb91728 327 //H(5,j+16) = -dcm(2,j);
NaotoMorita 70:d12e46fdc2f0 328 H(6,j+6) = rotmag(3,j)-(rotmag(3,j))/hz;
NaotoMorita 70:d12e46fdc2f0 329 }
NaotoMorita 70:d12e46fdc2f0 330 Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
NaotoMorita 70:d12e46fdc2f0 331 /*
NaotoMorita 70:d12e46fdc2f0 332 Matrix r3(3,1);
NaotoMorita 70:d12e46fdc2f0 333 r3 << tdcm(1,3)<< tdcm(2,3) << tdcm(3,3);
NaotoMorita 70:d12e46fdc2f0 334 Matrix kpart = r3*MatrixMath::Transpose(r3);
NaotoMorita 70:d12e46fdc2f0 335 Matrix Kmod(3,3);
NaotoMorita 70:d12e46fdc2f0 336 for(int i = 1; i<4; i++){
NaotoMorita 70:d12e46fdc2f0 337 for(int j = 1;j<4;j++){
NaotoMorita 70:d12e46fdc2f0 338 Kmod(i,j) = K(i+6,j+3);
NaotoMorita 70:d12e46fdc2f0 339 }
NaotoMorita 70:d12e46fdc2f0 340 }
NaotoMorita 70:d12e46fdc2f0 341 Kmod = kpart*Kmod;
NaotoMorita 70:d12e46fdc2f0 342 */
NaotoMorita 70:d12e46fdc2f0 343 for(int i = 1; i<nState+1; i++){
NaotoMorita 70:d12e46fdc2f0 344 for(int j = 4;j<7;j++){
NaotoMorita 70:d12e46fdc2f0 345 if(i>6 && i<10){
NaotoMorita 70:d12e46fdc2f0 346 //K(i,j) = Kmod(i-6,j-3);
NaotoMorita 70:d12e46fdc2f0 347 }else{
NaotoMorita 70:d12e46fdc2f0 348 K(i,j) = 0.0f;
NaotoMorita 70:d12e46fdc2f0 349 }
NaotoMorita 70:d12e46fdc2f0 350 }
NaotoMorita 65:c25d7810de44 351 }
NaotoMorita 65:c25d7810de44 352
NaotoMorita 65:c25d7810de44 353 Matrix zacc = -accm-tdcm*gravity;
NaotoMorita 70:d12e46fdc2f0 354 Matrix z(6,1);
NaotoMorita 70:d12e46fdc2f0 355 z << zacc(1,1) << zacc(2,1) << zacc(3,1) << -(magned(1,1) - hx) << -magned(2,1) << -(magned(3,1) - hz);
NaotoMorita 65:c25d7810de44 356 errState = K * z;
NaotoMorita 65:c25d7810de44 357 Phat = (MatrixMath::Eye(nState)-K*H)*Phat;
NaotoMorita 65:c25d7810de44 358
NaotoMorita 65:c25d7810de44 359 fuseErr2Nominal();
NaotoMorita 65:c25d7810de44 360 }
NaotoMorita 65:c25d7810de44 361
NaotoMorita 65:c25d7810de44 362
NaotoMorita 65:c25d7810de44 363 void solaESKF::updateOtherConstraints(Matrix mag,float palt,Matrix R)
NaotoMorita 65:c25d7810de44 364 {
NaotoMorita 65:c25d7810de44 365 Matrix magm = mag - magBias;
NaotoMorita 65:c25d7810de44 366 float a = sqrt(magm(1,1)*magm(1,1)+magm(2,1)*magm(2,1)+magm(3,1)*magm(3,1));
NaotoMorita 65:c25d7810de44 367 Matrix H(2,nState);
NaotoMorita 65:c25d7810de44 368 H(1,20) = -1.0f;
NaotoMorita 65:c25d7810de44 369 H(1,17) = -magm(1,1)/a;
NaotoMorita 65:c25d7810de44 370 H(1,18) = -magm(2,1)/a;
NaotoMorita 65:c25d7810de44 371 H(1,19) = -magm(3,1)/a;
NaotoMorita 65:c25d7810de44 372 H(2,3) = 1.0f;
NaotoMorita 65:c25d7810de44 373
NaotoMorita 65:c25d7810de44 374 Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
NaotoMorita 65:c25d7810de44 375
NaotoMorita 65:c25d7810de44 376 Matrix z(2,1);
NaotoMorita 65:c25d7810de44 377 z << -(a-magRadius) << palt-pihat(3,1);
NaotoMorita 65:c25d7810de44 378 //twelite.printf("%f %f\r\n",hx,(zmag(1,1) - hx));
NaotoMorita 46:15988dc41923 379 errState = K * z;
NaotoMorita 50:dadad0567349 380 Phat = (MatrixMath::Eye(nState)-K*H)*Phat;
NaotoMorita 58:93ba28cf5cb3 381
NaotoMorita 58:93ba28cf5cb3 382 fuseErr2Nominal();
NaotoMorita 58:93ba28cf5cb3 383 }
NaotoMorita 58:93ba28cf5cb3 384
NaotoMorita 58:93ba28cf5cb3 385
NaotoMorita 59:03fe5e16a33c 386 void solaESKF::updateGPS(Matrix posgps,float palt,Matrix velgps,Matrix R)
NaotoMorita 58:93ba28cf5cb3 387 {
NaotoMorita 59:03fe5e16a33c 388 Matrix H(5,nState);
NaotoMorita 46:15988dc41923 389 H(1,1) = 1.0f;
NaotoMorita 46:15988dc41923 390 H(2,2) = 1.0f;
NaotoMorita 56:c10f1168bd4a 391 H(3,3) = 1.0f;
NaotoMorita 56:c10f1168bd4a 392 H(4,4) = 1.0f;
NaotoMorita 56:c10f1168bd4a 393 H(5,5) = 1.0f;
NaotoMorita 53:8b551358a7e3 394 Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
NaotoMorita 59:03fe5e16a33c 395 Matrix z(5,1);
NaotoMorita 59:03fe5e16a33c 396 z << posgps(1,1) - pihat(1,1) << posgps(2,1)-pihat(2,1) << palt - pihat(3,1) << velgps(1,1) - vihat(1,1) << velgps(2,1)-vihat(2,1);
NaotoMorita 58:93ba28cf5cb3 397 errState = K * z;
NaotoMorita 58:93ba28cf5cb3 398 Phat = (MatrixMath::Eye(nState)-K*H)*Phat;
NaotoMorita 58:93ba28cf5cb3 399 fuseErr2Nominal();
NaotoMorita 58:93ba28cf5cb3 400 }
NaotoMorita 58:93ba28cf5cb3 401
NaotoMorita 46:15988dc41923 402
NaotoMorita 44:7d82e63b6a86 403 Matrix solaESKF::computeAngles()
NaotoMorita 25:07ac5c6cd61c 404 {
NaotoMorita 25:07ac5c6cd61c 405
NaotoMorita 44:7d82e63b6a86 406 Matrix euler(3,1);
NaotoMorita 44:7d82e63b6a86 407 euler(1,1) = atan2f(qhat(1,1)*qhat(2,1) + qhat(3,1)*qhat(4,1), 0.5f - qhat(2,1)*qhat(2,1) - qhat(3,1)*qhat(3,1));
NaotoMorita 44:7d82e63b6a86 408 euler(2,1) = asinf(-2.0f * (qhat(2,1)*qhat(4,1) - qhat(1,1)*qhat(3,1)));
NaotoMorita 44:7d82e63b6a86 409 euler(3,1) = atan2f(qhat(2,1)*qhat(3,1) + qhat(1,1)*qhat(4,1), 0.5f - qhat(3,1)*qhat(3,1) - qhat(4,1)*qhat(4,1));
NaotoMorita 44:7d82e63b6a86 410 return euler;
NaotoMorita 19:3fae66745363 411 }
NaotoMorita 19:3fae66745363 412
NaotoMorita 21:d6079def0473 413
NaotoMorita 44:7d82e63b6a86 414 void solaESKF::fuseErr2Nominal()
NaotoMorita 19:3fae66745363 415 {
NaotoMorita 44:7d82e63b6a86 416 //position
NaotoMorita 44:7d82e63b6a86 417 pihat(1,1) += errState(1,1);
NaotoMorita 44:7d82e63b6a86 418 pihat(2,1) += errState(2,1);
NaotoMorita 44:7d82e63b6a86 419 pihat(3,1) += errState(3,1);
NaotoMorita 44:7d82e63b6a86 420
NaotoMorita 44:7d82e63b6a86 421 //velocity
NaotoMorita 44:7d82e63b6a86 422 vihat(1,1) += errState(4,1);
NaotoMorita 44:7d82e63b6a86 423 vihat(2,1) += errState(5,1);
NaotoMorita 44:7d82e63b6a86 424 vihat(3,1) += errState(6,1);
NaotoMorita 44:7d82e63b6a86 425
NaotoMorita 44:7d82e63b6a86 426 //angle error
NaotoMorita 19:3fae66745363 427 Matrix qerr(4,1);
NaotoMorita 46:15988dc41923 428 qerr << 1.0f << 0.5f*errState(7,1) << 0.5f*errState(8,1) << 0.5f*errState(9,1);
NaotoMorita 19:3fae66745363 429 qhat = quatmultiply(qhat, qerr);
NaotoMorita 19:3fae66745363 430 float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qhat),qhat));
NaotoMorita 19:3fae66745363 431 qhat *= (1.0f/ qnorm);
NaotoMorita 23:1509648c2318 432
NaotoMorita 44:7d82e63b6a86 433 //acc bias
NaotoMorita 44:7d82e63b6a86 434 accBias(1,1) += errState(10,1);
NaotoMorita 44:7d82e63b6a86 435 accBias(2,1) += errState(11,1);
NaotoMorita 44:7d82e63b6a86 436 accBias(3,1) += errState(12,1);
NaotoMorita 44:7d82e63b6a86 437
NaotoMorita 44:7d82e63b6a86 438 //gyro bias
NaotoMorita 44:7d82e63b6a86 439 gyroBias(1,1) += errState(13,1);
NaotoMorita 44:7d82e63b6a86 440 gyroBias(2,1) += errState(14,1);
NaotoMorita 44:7d82e63b6a86 441 gyroBias(3,1) += errState(15,1);
NaotoMorita 44:7d82e63b6a86 442
NaotoMorita 44:7d82e63b6a86 443 //gravity bias
NaotoMorita 59:03fe5e16a33c 444 gravity(1,1) = 0.0f;
NaotoMorita 59:03fe5e16a33c 445 gravity(2,1) = 0.0f;
NaotoMorita 59:03fe5e16a33c 446 gravity(3,1) += errState(16,1);
NaotoMorita 70:d12e46fdc2f0 447
NaotoMorita 62:5519d34eb6e8 448
NaotoMorita 56:c10f1168bd4a 449
NaotoMorita 58:93ba28cf5cb3 450 for (int i = 1; i < nState+1; i++){
NaotoMorita 47:2467de40951f 451 errState(i,1) = 0.0f;
NaotoMorita 47:2467de40951f 452 }
NaotoMorita 44:7d82e63b6a86 453
NaotoMorita 19:3fae66745363 454 }
NaotoMorita 19:3fae66745363 455
NaotoMorita 44:7d82e63b6a86 456 Matrix solaESKF::quatmultiply(Matrix p, Matrix q)
NaotoMorita 19:3fae66745363 457 {
NaotoMorita 44:7d82e63b6a86 458
NaotoMorita 19:3fae66745363 459 Matrix qout(4,1);
NaotoMorita 44:7d82e63b6a86 460 qout(1,1) = p(1,1)*q(1,1) - p(2,1)*q(2,1) - p(3,1)*q(3,1) - p(4,1)*q(4,1);
NaotoMorita 44:7d82e63b6a86 461 qout(2,1) = p(1,1)*q(2,1) + p(2,1)*q(1,1) + p(3,1)*q(4,1) - p(4,1)*q(3,1);
NaotoMorita 44:7d82e63b6a86 462 qout(3,1) = p(1,1)*q(3,1) - p(2,1)*q(4,1) + p(3,1)*q(1,1) + p(4,1)*q(2,1);
NaotoMorita 44:7d82e63b6a86 463 qout(4,1) = p(1,1)*q(4,1) + p(2,1)*q(3,1) - p(3,1)*q(2,1) + p(4,1)*q(1,1);
NaotoMorita 19:3fae66745363 464 float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qout),qout));
NaotoMorita 19:3fae66745363 465 qout *= (1.0f/ qnorm);
NaotoMorita 19:3fae66745363 466 return qout;
NaotoMorita 19:3fae66745363 467 }
NaotoMorita 19:3fae66745363 468
NaotoMorita 44:7d82e63b6a86 469 void solaESKF::computeDcm(Matrix& dcm, Matrix quat)
NaotoMorita 19:3fae66745363 470 {
NaotoMorita 19:3fae66745363 471
NaotoMorita 44:7d82e63b6a86 472 dcm(1,1) = quat(1,1)*quat(1,1) + quat(2,1)*quat(2,1) - quat(3,1)*quat(3,1) - quat(4,1)*quat(4,1);
NaotoMorita 44:7d82e63b6a86 473 dcm(1,2) = 2.0f*(quat(2,1)*quat(3,1) - quat(1,1)*quat(4,1));
NaotoMorita 44:7d82e63b6a86 474 dcm(1,3) = 2.0f*(quat(2,1)*quat(4,1) + quat(1,1)*quat(3,1));
NaotoMorita 44:7d82e63b6a86 475 dcm(2,1) = 2.0f*(quat(2,1)*quat(3,1) + quat(1,1)*quat(4,1));
NaotoMorita 44:7d82e63b6a86 476 dcm(2,2) = quat(1,1)*quat(1,1) - quat(2,1)*quat(2,1) + quat(3,1)*quat(3,1) - quat(4,1)*quat(4,1);
NaotoMorita 44:7d82e63b6a86 477 dcm(2,3) = 2.0f*(quat(3,1)*quat(4,1) - quat(1,1)*quat(2,1));
NaotoMorita 44:7d82e63b6a86 478 dcm(3,1) = 2.0f*(quat(2,1)*quat(4,1) - quat(1,1)*quat(3,1));
NaotoMorita 44:7d82e63b6a86 479 dcm(3,2) = 2.0f*(quat(3,1)*quat(4,1) + quat(1,1)*quat(2,1));
NaotoMorita 44:7d82e63b6a86 480 dcm(3,3) = quat(1,1)*quat(1,1) - quat(2,1)*quat(2,1) - quat(3,1)*quat(3,1) + quat(4,1)*quat(4,1);
NaotoMorita 19:3fae66745363 481
NaotoMorita 19:3fae66745363 482 }
NaotoMorita 19:3fae66745363 483
NaotoMorita 44:7d82e63b6a86 484 void solaESKF::setQhat(float ex,float ey,float ez)
NaotoMorita 44:7d82e63b6a86 485 {
NaotoMorita 44:7d82e63b6a86 486 float cos_z_2 = cosf(0.5f*ez);
NaotoMorita 44:7d82e63b6a86 487 float cos_y_2 = cosf(0.5f*ey);
NaotoMorita 44:7d82e63b6a86 488 float cos_x_2 = cosf(0.5f*ex);
NaotoMorita 19:3fae66745363 489
NaotoMorita 44:7d82e63b6a86 490 float sin_z_2 = sinf(0.5f*ez);
NaotoMorita 44:7d82e63b6a86 491 float sin_y_2 = sinf(0.5f*ey);
NaotoMorita 44:7d82e63b6a86 492 float sin_x_2 = sinf(0.5f*ex);
NaotoMorita 19:3fae66745363 493
NaotoMorita 19:3fae66745363 494 // and now compute quaternion
NaotoMorita 19:3fae66745363 495 qhat(1,1) = cos_z_2*cos_y_2*cos_x_2 + sin_z_2*sin_y_2*sin_x_2;
NaotoMorita 19:3fae66745363 496 qhat(2,1) = cos_z_2*cos_y_2*sin_x_2 - sin_z_2*sin_y_2*cos_x_2;
NaotoMorita 19:3fae66745363 497 qhat(3,1) = cos_z_2*sin_y_2*cos_x_2 + sin_z_2*cos_y_2*sin_x_2;
NaotoMorita 19:3fae66745363 498 qhat(4,1) = sin_z_2*cos_y_2*cos_x_2 - cos_z_2*sin_y_2*sin_x_2;
NaotoMorita 19:3fae66745363 499 }
NaotoMorita 19:3fae66745363 500
NaotoMorita 47:2467de40951f 501
NaotoMorita 47:2467de40951f 502
NaotoMorita 44:7d82e63b6a86 503 void solaESKF::setGravity(float gx,float gy,float gz)
NaotoMorita 23:1509648c2318 504 {
NaotoMorita 44:7d82e63b6a86 505 gravity(1,1) = gx;
NaotoMorita 44:7d82e63b6a86 506 gravity(2,1) = gy;
NaotoMorita 44:7d82e63b6a86 507 gravity(3,1) = gz;
NaotoMorita 23:1509648c2318 508 }
NaotoMorita 62:5519d34eb6e8 509
NaotoMorita 47:2467de40951f 510 Matrix solaESKF::getPihat()
NaotoMorita 47:2467de40951f 511 {
NaotoMorita 47:2467de40951f 512 return pihat;
NaotoMorita 47:2467de40951f 513 }
NaotoMorita 47:2467de40951f 514 Matrix solaESKF::getVihat()
NaotoMorita 47:2467de40951f 515 {
NaotoMorita 47:2467de40951f 516 return vihat;
NaotoMorita 47:2467de40951f 517 }
NaotoMorita 47:2467de40951f 518 Matrix solaESKF::getQhat()
NaotoMorita 47:2467de40951f 519 {
NaotoMorita 47:2467de40951f 520 return qhat;
NaotoMorita 47:2467de40951f 521 }
NaotoMorita 47:2467de40951f 522 Matrix solaESKF::getAccBias()
NaotoMorita 47:2467de40951f 523 {
NaotoMorita 47:2467de40951f 524 return accBias;
NaotoMorita 47:2467de40951f 525 }
NaotoMorita 47:2467de40951f 526 Matrix solaESKF::getGyroBias()
NaotoMorita 47:2467de40951f 527 {
NaotoMorita 47:2467de40951f 528 return gyroBias;
NaotoMorita 47:2467de40951f 529 }
NaotoMorita 47:2467de40951f 530 Matrix solaESKF::getGravity()
NaotoMorita 47:2467de40951f 531 {
NaotoMorita 47:2467de40951f 532 return gravity;
NaotoMorita 47:2467de40951f 533 }
NaotoMorita 62:5519d34eb6e8 534 Matrix solaESKF::getMagBias()
NaotoMorita 62:5519d34eb6e8 535 {
NaotoMorita 62:5519d34eb6e8 536 return magBias;
NaotoMorita 62:5519d34eb6e8 537 }
NaotoMorita 62:5519d34eb6e8 538 float solaESKF::getMagRadius()
NaotoMorita 62:5519d34eb6e8 539 {
NaotoMorita 62:5519d34eb6e8 540 return magRadius;
NaotoMorita 62:5519d34eb6e8 541 }
NaotoMorita 47:2467de40951f 542 Matrix solaESKF::getErrState()
NaotoMorita 47:2467de40951f 543 {
NaotoMorita 47:2467de40951f 544 return errState;
NaotoMorita 47:2467de40951f 545 }
NaotoMorita 23:1509648c2318 546
NaotoMorita 47:2467de40951f 547 void solaESKF::setPhatPosition(float val)
NaotoMorita 47:2467de40951f 548 {
NaotoMorita 47:2467de40951f 549 setBlockDiag(Phat,val,1,3);
NaotoMorita 47:2467de40951f 550 }
NaotoMorita 47:2467de40951f 551 void solaESKF::setPhatVelocity(float val)
NaotoMorita 47:2467de40951f 552 {
NaotoMorita 47:2467de40951f 553 setBlockDiag(Phat,val,4,6);
NaotoMorita 47:2467de40951f 554 }
NaotoMorita 47:2467de40951f 555 void solaESKF::setPhatAngleError(float val)
NaotoMorita 47:2467de40951f 556 {
NaotoMorita 47:2467de40951f 557 setBlockDiag(Phat,val,7,9);
NaotoMorita 47:2467de40951f 558 }
NaotoMorita 47:2467de40951f 559 void solaESKF::setPhatAccBias(float val)
NaotoMorita 47:2467de40951f 560 {
NaotoMorita 47:2467de40951f 561 setBlockDiag(Phat,val,10,12);
NaotoMorita 47:2467de40951f 562 }
NaotoMorita 47:2467de40951f 563 void solaESKF::setPhatGyroBias(float val)
NaotoMorita 47:2467de40951f 564 {
NaotoMorita 47:2467de40951f 565 setBlockDiag(Phat,val,13,15);
NaotoMorita 47:2467de40951f 566 }
NaotoMorita 47:2467de40951f 567 void solaESKF::setPhatGravity(float val)
NaotoMorita 47:2467de40951f 568 {
NaotoMorita 59:03fe5e16a33c 569 setBlockDiag(Phat,val,16,16);
NaotoMorita 47:2467de40951f 570 }
NaotoMorita 62:5519d34eb6e8 571 void solaESKF::setPhatMagBias(float val)
NaotoMorita 62:5519d34eb6e8 572 {
NaotoMorita 62:5519d34eb6e8 573 setBlockDiag(Phat,val,17,19);
NaotoMorita 62:5519d34eb6e8 574 }
NaotoMorita 62:5519d34eb6e8 575 void solaESKF::setPhatMagRadius(float val)
NaotoMorita 62:5519d34eb6e8 576 {
NaotoMorita 62:5519d34eb6e8 577 setBlockDiag(Phat,val,20,20);
NaotoMorita 62:5519d34eb6e8 578 }
NaotoMorita 47:2467de40951f 579
NaotoMorita 47:2467de40951f 580
NaotoMorita 47:2467de40951f 581 void solaESKF::setQVelocity(float val)
NaotoMorita 47:2467de40951f 582 {
NaotoMorita 47:2467de40951f 583 setBlockDiag(Q,val,4,6);
NaotoMorita 47:2467de40951f 584 }
NaotoMorita 47:2467de40951f 585 void solaESKF::setQAngleError(float val)
NaotoMorita 47:2467de40951f 586 {
NaotoMorita 47:2467de40951f 587 setBlockDiag(Q,val,7,9);
NaotoMorita 47:2467de40951f 588 }
NaotoMorita 47:2467de40951f 589 void solaESKF::setQAccBias(float val)
NaotoMorita 47:2467de40951f 590 {
NaotoMorita 47:2467de40951f 591 setBlockDiag(Q,val,10,12);
NaotoMorita 47:2467de40951f 592 }
NaotoMorita 47:2467de40951f 593 void solaESKF::setQGyroBias(float val)
NaotoMorita 47:2467de40951f 594 {
NaotoMorita 47:2467de40951f 595 setBlockDiag(Q,val,13,15);
NaotoMorita 47:2467de40951f 596 }
NaotoMorita 62:5519d34eb6e8 597 void solaESKF::setQMagBias(float val)
NaotoMorita 62:5519d34eb6e8 598 {
NaotoMorita 62:5519d34eb6e8 599 setBlockDiag(Q,val,17,19);
NaotoMorita 62:5519d34eb6e8 600 }
NaotoMorita 62:5519d34eb6e8 601 void solaESKF::setQMagRadius(float val)
NaotoMorita 62:5519d34eb6e8 602 {
NaotoMorita 62:5519d34eb6e8 603 setBlockDiag(Q,val,20,20);
NaotoMorita 62:5519d34eb6e8 604 }
NaotoMorita 19:3fae66745363 605
NaotoMorita 19:3fae66745363 606
NaotoMorita 44:7d82e63b6a86 607 void solaESKF::setDiag(Matrix& mat, float val){
NaotoMorita 44:7d82e63b6a86 608 for (int i = 1; i < mat.getCols()+1; i++){
NaotoMorita 44:7d82e63b6a86 609 mat(i,i) = val;
NaotoMorita 22:7d84b8bc20b4 610 }
NaotoMorita 22:7d84b8bc20b4 611 }
NaotoMorita 22:7d84b8bc20b4 612
NaotoMorita 45:df4618814803 613 void solaESKF::setBlockDiag(Matrix& mat, float val,int startIndex, int endIndex){
NaotoMorita 44:7d82e63b6a86 614 for (int i = startIndex; i < endIndex+1; i++){
NaotoMorita 44:7d82e63b6a86 615 mat(i,i) = val;
NaotoMorita 22:7d84b8bc20b4 616 }
NaotoMorita 22:7d84b8bc20b4 617 }
osaka 51:a5af3b280d23 618
NaotoMorita 58:93ba28cf5cb3 619 void solaESKF::setPihat(float pi_x, float pi_y)
osaka 51:a5af3b280d23 620 {
NaotoMorita 56:c10f1168bd4a 621 pihat(1,1) = pi_x;
NaotoMorita 56:c10f1168bd4a 622 pihat(2,1) = pi_y;
NaotoMorita 58:93ba28cf5cb3 623 //pihat(3,1) = pi_z;
NaotoMorita 56:c10f1168bd4a 624 }
NaotoMorita 73:5770a0d470c0 625
NaotoMorita 73:5770a0d470c0 626 float solaESKF::wrap_pi(float x) {
NaotoMorita 73:5770a0d470c0 627 const float pi = 3.141592f;
NaotoMorita 73:5770a0d470c0 628 const float range = pi + pi;
NaotoMorita 73:5770a0d470c0 629 twelite.printf("1 : %f ",x);
NaotoMorita 73:5770a0d470c0 630
NaotoMorita 73:5770a0d470c0 631 if (x < -pi) {
NaotoMorita 73:5770a0d470c0 632 x += range * ((-pi - x) / range + 1);
NaotoMorita 73:5770a0d470c0 633 }
NaotoMorita 73:5770a0d470c0 634
NaotoMorita 73:5770a0d470c0 635 twelite.printf("2 : %f\r\n",-pi + fmodf( (x + pi),range));
NaotoMorita 73:5770a0d470c0 636 return -pi + fmodf((x + pi),range);
NaotoMorita 73:5770a0d470c0 637 }
NaotoMorita 56:c10f1168bd4a 638 /*
NaotoMorita 56:c10f1168bd4a 639 void solaESKF::updateAccConstraints(Matrix acc,float palt,Matrix R)
NaotoMorita 56:c10f1168bd4a 640 {
NaotoMorita 56:c10f1168bd4a 641 Matrix accm = acc - accBias;
NaotoMorita 56:c10f1168bd4a 642 Matrix tdcm(3,3);
NaotoMorita 56:c10f1168bd4a 643 computeDcm(tdcm, qhat);
NaotoMorita 56:c10f1168bd4a 644 tdcm = MatrixMath::Transpose(tdcm);
NaotoMorita 56:c10f1168bd4a 645 Matrix tdcm_g = tdcm*gravity;
NaotoMorita 56:c10f1168bd4a 646 Matrix a2v = MatrixMath::Matrixcross(tdcm_g(1,1),tdcm_g(2,1),tdcm_g(3,1));
NaotoMorita 56:c10f1168bd4a 647
NaotoMorita 56:c10f1168bd4a 648 Matrix H(4,nState);
NaotoMorita 56:c10f1168bd4a 649 for (int i = 1; i < 4; i++){
NaotoMorita 56:c10f1168bd4a 650 for (int j = 1; j < 4; j++){
NaotoMorita 56:c10f1168bd4a 651 H(i,j+6) = a2v(i,j);
NaotoMorita 56:c10f1168bd4a 652 H(i,j+15) = tdcm(i,j);
NaotoMorita 56:c10f1168bd4a 653 }
NaotoMorita 56:c10f1168bd4a 654 }
NaotoMorita 56:c10f1168bd4a 655 H(1,10) = -1.0f;
NaotoMorita 56:c10f1168bd4a 656 H(2,11) = -1.0f;
NaotoMorita 56:c10f1168bd4a 657 H(3,12) = -1.0f;
NaotoMorita 56:c10f1168bd4a 658 H(4,3) = 1.0f;
NaotoMorita 56:c10f1168bd4a 659
NaotoMorita 56:c10f1168bd4a 660 //Matrix A = H*Phat*MatrixMath::Transpose(H)+R;
NaotoMorita 56:c10f1168bd4a 661 //Matrix K = (Phat*MatrixMath::Transpose(H))*(MatrixMath::Inv(MatrixMath::Transpose(A)*A+10.0f*MatrixMath::Eye(3))*MatrixMath::Transpose(A));
NaotoMorita 56:c10f1168bd4a 662 Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
NaotoMorita 56:c10f1168bd4a 663 Matrix zacc = -accm-tdcm*gravity;
NaotoMorita 56:c10f1168bd4a 664 Matrix z(4,1);
NaotoMorita 56:c10f1168bd4a 665 z << zacc(1,1) << zacc(2,1) << zacc(3,1) << palt - pihat(3,1);
NaotoMorita 56:c10f1168bd4a 666 errState = K * z;
NaotoMorita 56:c10f1168bd4a 667 Phat = (MatrixMath::Eye(nState)-K*H)*Phat;
NaotoMorita 56:c10f1168bd4a 668 //Phat = Phat-K*(H*Phat*MatrixMath::Transpose(H)+R)*MatrixMath::Transpose(K);
NaotoMorita 56:c10f1168bd4a 669 //Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*R*MatrixMath::Transpose(K);
NaotoMorita 56:c10f1168bd4a 670 fuseErr2Nominal();
NaotoMorita 56:c10f1168bd4a 671 }
NaotoMorita 56:c10f1168bd4a 672
NaotoMorita 56:c10f1168bd4a 673 void solaESKF::updateGyroConstraints(Matrix gyro,Matrix R)
NaotoMorita 56:c10f1168bd4a 674 {
NaotoMorita 56:c10f1168bd4a 675 Matrix gyrom = gyro - gyroBias;
NaotoMorita 56:c10f1168bd4a 676 Matrix dcm(3,3);
NaotoMorita 56:c10f1168bd4a 677 computeDcm(dcm, qhat);
NaotoMorita 56:c10f1168bd4a 678 Matrix a2v = dcm*MatrixMath::Matrixcross(gyrom(1,1),gyrom(2,1),gyrom(3,1));
NaotoMorita 56:c10f1168bd4a 679
NaotoMorita 56:c10f1168bd4a 680 Matrix H(2,nState);
NaotoMorita 56:c10f1168bd4a 681 for (int i = 1; i < 3; i++){
NaotoMorita 56:c10f1168bd4a 682 for (int j = 1; j < 4; j++){
NaotoMorita 56:c10f1168bd4a 683 H(i,j+6) = a2v(i,j);
NaotoMorita 56:c10f1168bd4a 684 H(i,j+12) = -dcm(i,j);
NaotoMorita 56:c10f1168bd4a 685 }
NaotoMorita 56:c10f1168bd4a 686 }
NaotoMorita 56:c10f1168bd4a 687 //Matrix A = H*Phat*MatrixMath::Transpose(H)+R;
NaotoMorita 56:c10f1168bd4a 688 //Matrix K = (Phat*MatrixMath::Transpose(H))*(MatrixMath::Inv(MatrixMath::Transpose(A)*A+10.0f*MatrixMath::Eye(2))*MatrixMath::Transpose(A));
NaotoMorita 56:c10f1168bd4a 689 Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
NaotoMorita 56:c10f1168bd4a 690
NaotoMorita 56:c10f1168bd4a 691 Matrix z3 = -dcm*gyrom;
NaotoMorita 56:c10f1168bd4a 692 Matrix z(2,1);
NaotoMorita 56:c10f1168bd4a 693 z << z3(1,1) << z3(2,1);
NaotoMorita 56:c10f1168bd4a 694 errState = K * z;
NaotoMorita 56:c10f1168bd4a 695 //Phat = (MatrixMath::Eye(nState)-K*H)*Phat;
NaotoMorita 56:c10f1168bd4a 696 //Phat = Phat-K*(H*Phat*MatrixMath::Transpose(H)+R)*MatrixMath::Transpose(K);
NaotoMorita 56:c10f1168bd4a 697 Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*R*MatrixMath::Transpose(K);
NaotoMorita 56:c10f1168bd4a 698 fuseErr2Nominal();
NaotoMorita 56:c10f1168bd4a 699 }
NaotoMorita 56:c10f1168bd4a 700 void solaESKF::updateMag(Matrix mag, Matrix R)
NaotoMorita 56:c10f1168bd4a 701 {
NaotoMorita 56:c10f1168bd4a 702 Matrix dcm(3,3);
NaotoMorita 56:c10f1168bd4a 703 computeDcm(dcm, qhat);
NaotoMorita 56:c10f1168bd4a 704 Matrix a2v = -dcm*MatrixMath::Matrixcross(mag(1,1),mag(2,1),mag(3,1));
NaotoMorita 56:c10f1168bd4a 705
NaotoMorita 56:c10f1168bd4a 706 Matrix H(2,nState);
NaotoMorita 56:c10f1168bd4a 707 for (int i = 1; i < 3; i++){
NaotoMorita 56:c10f1168bd4a 708 for (int j = 1; j < 4; j++){
NaotoMorita 56:c10f1168bd4a 709 H(i,j+6) = a2v(i,j);
NaotoMorita 56:c10f1168bd4a 710 }
NaotoMorita 56:c10f1168bd4a 711 }
NaotoMorita 56:c10f1168bd4a 712 H(1,19) = 1.0f;
NaotoMorita 56:c10f1168bd4a 713 //H(3,20) = 1.0f;
NaotoMorita 56:c10f1168bd4a 714
NaotoMorita 56:c10f1168bd4a 715 //Matrix A = H*Phat*MatrixMath::Transpose(H)+R;
NaotoMorita 56:c10f1168bd4a 716 //Matrix K = (Phat*MatrixMath::Transpose(H))*(MatrixMath::Inv(MatrixMath::Transpose(A)*A+10.0f*MatrixMath::Eye(3))*MatrixMath::Transpose(A));
NaotoMorita 56:c10f1168bd4a 717 Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
NaotoMorita 56:c10f1168bd4a 718 Matrix zmag = -dcm*mag-magField;
NaotoMorita 56:c10f1168bd4a 719 Matrix z(2,1);
NaotoMorita 56:c10f1168bd4a 720 z << zmag(1,1) << zmag(2,1);
NaotoMorita 56:c10f1168bd4a 721 errState = K * z;
NaotoMorita 56:c10f1168bd4a 722 Phat = (MatrixMath::Eye(nState)-K*H)*Phat;
NaotoMorita 56:c10f1168bd4a 723 //Phat = Phat-K*(H*Phat*MatrixMath::Transpose(H)+R)*MatrixMath::Transpose(K);
NaotoMorita 56:c10f1168bd4a 724 //Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*R*MatrixMath::Transpose(K);
NaotoMorita 56:c10f1168bd4a 725 fuseErr2Nominal();
NaotoMorita 56:c10f1168bd4a 726 }
NaotoMorita 59:03fe5e16a33c 727 void solaESKF::updateMag(Matrix mag,float palt, Matrix R)
NaotoMorita 59:03fe5e16a33c 728 {
NaotoMorita 59:03fe5e16a33c 729 Matrix dcm(3,3);
NaotoMorita 59:03fe5e16a33c 730 computeDcm(dcm, qhat);
NaotoMorita 59:03fe5e16a33c 731 Matrix a2v = -dcm*MatrixMath::Matrixcross(mag(1,1),mag(2,1),mag(3,1));
NaotoMorita 59:03fe5e16a33c 732
NaotoMorita 59:03fe5e16a33c 733 Matrix H(3,nState);
NaotoMorita 59:03fe5e16a33c 734 for (int i = 1; i < 3; i++){
NaotoMorita 59:03fe5e16a33c 735 for (int j = 1; j < 4; j++){
NaotoMorita 59:03fe5e16a33c 736 H(i,j+6) = a2v(i,j);
NaotoMorita 59:03fe5e16a33c 737 }
NaotoMorita 59:03fe5e16a33c 738 }
NaotoMorita 59:03fe5e16a33c 739 H(1,19) = 1.0f;
NaotoMorita 59:03fe5e16a33c 740 H(3,3) = 1.0f;
NaotoMorita 59:03fe5e16a33c 741 //Matrix A = H*Phat*MatrixMath::Transpose(H)+R;
NaotoMorita 59:03fe5e16a33c 742 //Matrix K = (Phat*MatrixMath::Transpose(H))*(MatrixMath::Inv(MatrixMath::Transpose(A)*A+10.0f*MatrixMath::Eye(3))*MatrixMath::Transpose(A));
NaotoMorita 59:03fe5e16a33c 743 Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
NaotoMorita 59:03fe5e16a33c 744 Matrix zmag = -dcm*mag-magField;
NaotoMorita 59:03fe5e16a33c 745 Matrix z(3,1);
NaotoMorita 59:03fe5e16a33c 746 z << zmag(1,1) << zmag(2,1) << palt - pihat(3,1);
NaotoMorita 59:03fe5e16a33c 747 errState = K * z;
NaotoMorita 59:03fe5e16a33c 748 Phat = (MatrixMath::Eye(nState)-K*H)*Phat;
NaotoMorita 59:03fe5e16a33c 749 //Phat = Phat-K*(H*Phat*MatrixMath::Transpose(H)+R)*MatrixMath::Transpose(K);
NaotoMorita 59:03fe5e16a33c 750 //Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*R*MatrixMath::Transpose(K);
NaotoMorita 59:03fe5e16a33c 751
NaotoMorita 59:03fe5e16a33c 752 fuseErr2Nominal();
NaotoMorita 59:03fe5e16a33c 753 }
NaotoMorita 59:03fe5e16a33c 754 void solaESKF::updateGPSVelocity(Matrix velgps,Matrix mag,Matrix R)
NaotoMorita 59:03fe5e16a33c 755 {
NaotoMorita 59:03fe5e16a33c 756
NaotoMorita 59:03fe5e16a33c 757 Matrix dcm(3,3);
NaotoMorita 59:03fe5e16a33c 758 computeDcm(dcm, qhat);
NaotoMorita 59:03fe5e16a33c 759 Matrix a2v = -dcm*MatrixMath::Matrixcross(mag(1,1),mag(2,1),mag(3,1));
NaotoMorita 59:03fe5e16a33c 760
NaotoMorita 59:03fe5e16a33c 761
NaotoMorita 59:03fe5e16a33c 762 Matrix H(3,nState);
NaotoMorita 59:03fe5e16a33c 763 H(1,4) = 1.0f;
NaotoMorita 59:03fe5e16a33c 764 H(2,5) = 1.0f;
NaotoMorita 59:03fe5e16a33c 765
NaotoMorita 59:03fe5e16a33c 766 for (int j = 1; j < 4; j++){
NaotoMorita 59:03fe5e16a33c 767 H(3,j+6) = a2v(2,j);
NaotoMorita 59:03fe5e16a33c 768 }
NaotoMorita 59:03fe5e16a33c 769 //H(3,19) = 1.0f;
NaotoMorita 59:03fe5e16a33c 770
NaotoMorita 59:03fe5e16a33c 771 Matrix zmag = -dcm*mag-magField;
NaotoMorita 59:03fe5e16a33c 772
NaotoMorita 59:03fe5e16a33c 773 //Matrix A = H*Phat*MatrixMath::Transpose(H)+R;
NaotoMorita 59:03fe5e16a33c 774 //Matrix K = (Phat*MatrixMath::Transpose(H))*(MatrixMath::Inv(MatrixMath::Transpose(A)*A+10.0f*MatrixMath::Eye(3))*MatrixMath::Transpose(A));
NaotoMorita 59:03fe5e16a33c 775 Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
NaotoMorita 59:03fe5e16a33c 776 Matrix z(3,1);
NaotoMorita 59:03fe5e16a33c 777 z << velgps(1,1) - vihat(1,1) << velgps(2,1)-vihat(2,1) << zmag(2,1);
NaotoMorita 59:03fe5e16a33c 778 errState = K * z;
NaotoMorita 59:03fe5e16a33c 779 Phat = (MatrixMath::Eye(nState)-K*H)*Phat;
NaotoMorita 59:03fe5e16a33c 780 //Phat = Phat-K*(H*Phat*MatrixMath::Transpose(H)+R)*MatrixMath::Transpose(K);
NaotoMorita 59:03fe5e16a33c 781 //Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*R*MatrixMath::Transpose(K);
NaotoMorita 59:03fe5e16a33c 782 fuseErr2Nominal();
NaotoMorita 59:03fe5e16a33c 783 }
NaotoMorita 56:c10f1168bd4a 784
NaotoMorita 59:03fe5e16a33c 785 void solaESKF::updateGPSPosition(Matrix posgps,float palt,Matrix R)
NaotoMorita 59:03fe5e16a33c 786 {
NaotoMorita 59:03fe5e16a33c 787 Matrix H(3,nState);
NaotoMorita 59:03fe5e16a33c 788 H(1,1) = 1.0f;
NaotoMorita 59:03fe5e16a33c 789 H(2,2) = 1.0f;
NaotoMorita 59:03fe5e16a33c 790 H(3,3) = 1.0f;
NaotoMorita 59:03fe5e16a33c 791
NaotoMorita 59:03fe5e16a33c 792 //Matrix A = H*Phat*MatrixMath::Transpose(H)+R;
NaotoMorita 59:03fe5e16a33c 793 //Matrix K = (Phat*MatrixMath::Transpose(H))*(MatrixMath::Inv(MatrixMath::Transpose(A)*A+1000.0f*MatrixMath::Eye(3))*MatrixMath::Transpose(A));
NaotoMorita 59:03fe5e16a33c 794 Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
NaotoMorita 59:03fe5e16a33c 795 Matrix z(3,1);
NaotoMorita 59:03fe5e16a33c 796 z << posgps(1,1) - pihat(1,1) << posgps(2,1)-pihat(2,1) << palt - pihat(3,1);
NaotoMorita 59:03fe5e16a33c 797 errState = K * z;
NaotoMorita 59:03fe5e16a33c 798 Phat = (MatrixMath::Eye(nState)-K*H)*Phat;
NaotoMorita 59:03fe5e16a33c 799 //Phat = Phat-K*(H*Phat*MatrixMath::Transpose(H)+R)*MatrixMath::Transpose(K);
NaotoMorita 59:03fe5e16a33c 800 //Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*R*MatrixMath::Transpose(K);
NaotoMorita 59:03fe5e16a33c 801 fuseErr2Nominal();
NaotoMorita 59:03fe5e16a33c 802 }
NaotoMorita 61:5e5c4fe12440 803
NaotoMorita 61:5e5c4fe12440 804 void solaESKF::updateImuConstraints(Matrix acc,Matrix mag,Matrix R)
NaotoMorita 61:5e5c4fe12440 805 {
NaotoMorita 61:5e5c4fe12440 806 Matrix accm = acc - accBias;
NaotoMorita 61:5e5c4fe12440 807 Matrix magm = mag - magBias;
NaotoMorita 61:5e5c4fe12440 808 Matrix dcm(3,3);
NaotoMorita 61:5e5c4fe12440 809 computeDcm(dcm, qhat);
NaotoMorita 61:5e5c4fe12440 810 Matrix tdcm = MatrixMath::Transpose(dcm);
NaotoMorita 61:5e5c4fe12440 811 Matrix tdcm_g = tdcm*gravity;
NaotoMorita 61:5e5c4fe12440 812 Matrix rotgrav = MatrixMath::Matrixcross(tdcm_g(1,1),tdcm_g(2,1),tdcm_g(3,1));
NaotoMorita 61:5e5c4fe12440 813 Matrix rotmag = -dcm*MatrixMath::Matrixcross(magm(1,1),magm(2,1),magm(3,1));
NaotoMorita 61:5e5c4fe12440 814
NaotoMorita 61:5e5c4fe12440 815 Matrix H(5,nState);
NaotoMorita 61:5e5c4fe12440 816 for (int i = 1; i < 4; i++){
NaotoMorita 61:5e5c4fe12440 817 for (int j = 1; j < 4; j++){
NaotoMorita 61:5e5c4fe12440 818 H(i,j+6) = rotgrav(i,j);
NaotoMorita 61:5e5c4fe12440 819 }
NaotoMorita 61:5e5c4fe12440 820 H(i,16) = tdcm(i,3);
NaotoMorita 61:5e5c4fe12440 821 }
NaotoMorita 61:5e5c4fe12440 822
NaotoMorita 61:5e5c4fe12440 823 H(1,10) = -1.0f;
NaotoMorita 61:5e5c4fe12440 824 H(2,11) = -1.0f;
NaotoMorita 61:5e5c4fe12440 825 H(3,12) = -1.0f;
NaotoMorita 61:5e5c4fe12440 826
NaotoMorita 61:5e5c4fe12440 827 Matrix magned = dcm*magm;
NaotoMorita 61:5e5c4fe12440 828 float hx = sqrt(magned(1,1)*magned(1,1)+magned(2,1)*magned(2,1));
NaotoMorita 61:5e5c4fe12440 829
NaotoMorita 61:5e5c4fe12440 830 for(int j = 1; j < 4; j++){
NaotoMorita 61:5e5c4fe12440 831 H(4,j+6) = rotmag(1,j)-(rotmag(1,j)+rotmag(2,j))/hx;
NaotoMorita 61:5e5c4fe12440 832 H(4,j+16) = -dcm(1,j)+(dcm(1,j)+dcm(2,j))/hx;
NaotoMorita 61:5e5c4fe12440 833 H(5,j+6) = rotmag(2,j);
NaotoMorita 61:5e5c4fe12440 834 H(5,j+16) = -dcm(2,j);
NaotoMorita 61:5e5c4fe12440 835 }
NaotoMorita 61:5e5c4fe12440 836
NaotoMorita 61:5e5c4fe12440 837 Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
NaotoMorita 61:5e5c4fe12440 838 Matrix zacc = -accm-tdcm*gravity;
NaotoMorita 61:5e5c4fe12440 839 Matrix zmag = dcm*magm;
NaotoMorita 61:5e5c4fe12440 840 Matrix z(5,1);
NaotoMorita 61:5e5c4fe12440 841 z << zacc(1,1) << zacc(2,1) << zacc(3,1) << -(zmag(1,1) - hx) << -zmag(2,1);
NaotoMorita 61:5e5c4fe12440 842 twelite.printf("%f %f\r\n",hx,(zmag(1,1) - hx));
NaotoMorita 61:5e5c4fe12440 843 errState = K * z;
NaotoMorita 61:5e5c4fe12440 844 Phat = (MatrixMath::Eye(nState)-K*H)*Phat;
NaotoMorita 61:5e5c4fe12440 845
NaotoMorita 61:5e5c4fe12440 846 fuseErr2Nominal();
NaotoMorita 61:5e5c4fe12440 847 }
NaotoMorita 73:5770a0d470c0 848 float q0 = qhat(1,1);
NaotoMorita 73:5770a0d470c0 849 float q1 = qhat(2,1);
NaotoMorita 73:5770a0d470c0 850 float q2 = qhat(3,1);
NaotoMorita 73:5770a0d470c0 851 float q3 = qhat(4,1);
NaotoMorita 73:5770a0d470c0 852
NaotoMorita 73:5770a0d470c0 853 float d0 = (-q3*q3-q2*q2+q1*q1+q0*q0);
NaotoMorita 73:5770a0d470c0 854 float q0q3q1q2 = (q0*q3+q1*q2);
NaotoMorita 73:5770a0d470c0 855 float h1lower = 2.0f*(4.0f*q0q3q1q2*q0q3q1q2/d0/d0+1.0f)*sqrt(4.0f*q0q3q1q2*q0q3q1q2/d0/d0+1.0f);
NaotoMorita 73:5770a0d470c0 856
NaotoMorita 73:5770a0d470c0 857 float d1 = d0*sqrt(4.0f*q0q3q1q2*q0q3q1q2/d0/d0+1.0f);
NaotoMorita 73:5770a0d470c0 858 float d2 = d0*d0*sqrt(4.0f*q0q3q1q2*q0q3q1q2/d0/d0+1.0f);
NaotoMorita 73:5770a0d470c0 859 float d3 = d0*sqrt(4.0f*q0q3q1q2*q0q3q1q2/d0/d0+1.0f)*(4.0f*q0q3q1q2*q0q3q1q2/d0/d0+1.0f);
NaotoMorita 73:5770a0d470c0 860
NaotoMorita 73:5770a0d470c0 861
NaotoMorita 73:5770a0d470c0 862
NaotoMorita 73:5770a0d470c0 863 Matrix Hh(2,4);
NaotoMorita 73:5770a0d470c0 864 Hh(1,1) = -(8.0f*q3*q0q3q1q2/d0/d0-16.0f*q0*q0q3q1q2*q0q3q1q2/d0/d0/d0)/h1lower;
NaotoMorita 73:5770a0d470c0 865 Hh(1,2) = -(8.0f*q2*q0q3q1q2/d0/d0-16.0f*q1*q0q3q1q2*q0q3q1q2/d0/d0/d0)/h1lower;
NaotoMorita 73:5770a0d470c0 866 Hh(1,3) = -(8.0f*q1*q0q3q1q2/d0/d0-16.0f*q2*q0q3q1q2*q0q3q1q2/d0/d0/d0)/h1lower;
NaotoMorita 73:5770a0d470c0 867 Hh(1,4) = -(8.0f*q0*q0q3q1q2/d0/d0-16.0f*q3*q0q3q1q2*q0q3q1q2/d0/d0/d0)/h1lower;
NaotoMorita 73:5770a0d470c0 868
NaotoMorita 73:5770a0d470c0 869 Hh(2,1) = 2.0f*q3/d1-4.0f*q0*q0q3q1q2/d2-q0q3q1q2*(8.0f*q3*q0q3q1q2/d0/d0-16.0f*q0*q0q3q1q2*q0q3q1q2/d0/d0/d0)/d3;
NaotoMorita 73:5770a0d470c0 870 Hh(2,2) = 2.0f*q2/d1-4.0f*q1*q0q3q1q2/d2-q0q3q1q2*(8.0f*q2*q0q3q1q2/d0/d0-16.0f*q1*q0q3q1q2*q0q3q1q2/d0/d0/d0)/d3;
NaotoMorita 73:5770a0d470c0 871 Hh(2,3) = 2.0f*q1/d1+4.0f*q2*q0q3q1q2/d2-q0q3q1q2*(8.0f*q1*q0q3q1q2/d0/d0-16.0f*q2*q0q3q1q2*q0q3q1q2/d0/d0/d0)/d3;
NaotoMorita 73:5770a0d470c0 872 Hh(2,4) = 2.0f*q0/d1+4.0f*q3*q0q3q1q2/d2-q0q3q1q2*(8.0f*q0*q0q3q1q2/d0/d0-16.0f*q3*q0q3q1q2*q0q3q1q2/d0/d0/d0)/d3;
NaotoMorita 56:c10f1168bd4a 873 */