HAPSRG / solaESKF_Eigen

Eigen

Dependencies:   Eigen

Dependents:   optWingforHAPS_Eigen hexaTest_Eigen

solaESKF.cpp@62:5519d34eb6e8, 2021-11-16 (annotated)

Committer:
NaotoMorita
Date:
Tue Nov 16 14:17:40 2021 +0000
Revision:
62:5519d34eb6e8
Parent:
61:5e5c4fe12440
Child:
63:74a9565a0141
magRadius Estimate

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 62:5519d34eb6e8 6 :pihat(3,1),vihat(3,1),qhat(4,1),accBias(3,1),gyroBias(3,1),gravity(3,1),magBias(3,1),errState(20,1),Phat(20,20),Q(20,20)
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 62:5519d34eb6e8 33 setBlockDiag(Phat,0.001f,17,19);//magBias
NaotoMorita 62:5519d34eb6e8 34 setBlockDiag(Phat,0.001f,20,20);//magRadius
NaotoMorita 47:2467de40951f 35 setBlockDiag(Q,0.00025f,4,6);//velocity
NaotoMorita 55:21611d4cf7e8 36 setBlockDiag(Q,0.005f/57.0f,7,9);//angle error
NaotoMorita 46:15988dc41923 37 setBlockDiag(Q,0.001f,10,12);//acc bias
NaotoMorita 62:5519d34eb6e8 38 setBlockDiag(Q,0.001f,13,15);//gyro bias
NaotoMorita 62:5519d34eb6e8 39 setBlockDiag(Q,0.001f,17,19);//magRadius
NaotoMorita 62:5519d34eb6e8 40 setBlockDiag(Q,0.0001f,20,20);//magRadius //positionとgravityはQ項なし
NaotoMorita 19:3fae66745363 41
NaotoMorita 19:3fae66745363 42 }
NaotoMorita 19:3fae66745363 43
NaotoMorita 47:2467de40951f 44
NaotoMorita 44:7d82e63b6a86 45 void solaESKF::updateNominal(Matrix acc, Matrix gyro,float att_dt)
NaotoMorita 19:3fae66745363 46 {
NaotoMorita 44:7d82e63b6a86 47 Matrix gyrom = gyro - gyroBias;
NaotoMorita 44:7d82e63b6a86 48 Matrix accm = acc - accBias;
NaotoMorita 44:7d82e63b6a86 49
NaotoMorita 44:7d82e63b6a86 50 Matrix qint(4,1);
NaotoMorita 46:15988dc41923 51 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 52 qhat = quatmultiply(qhat,qint);
NaotoMorita 19:3fae66745363 53 float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qhat),qhat));
NaotoMorita 19:3fae66745363 54 qhat *= (1.0f/ qnorm);
NaotoMorita 23:1509648c2318 55
NaotoMorita 23:1509648c2318 56 Matrix dcm(3,3);
NaotoMorita 23:1509648c2318 57 computeDcm(dcm, qhat);
NaotoMorita 46:15988dc41923 58 Matrix accned = dcm*accm+gravity;
NaotoMorita 44:7d82e63b6a86 59 vihat += accned*att_dt;
NaotoMorita 44:7d82e63b6a86 60
NaotoMorita 44:7d82e63b6a86 61 pihat += vihat*att_dt+0.5f*accned*att_dt*att_dt;
NaotoMorita 19:3fae66745363 62 }
NaotoMorita 19:3fae66745363 63
NaotoMorita 44:7d82e63b6a86 64 void solaESKF::updateErrState(Matrix acc,Matrix gyro,float att_dt)
NaotoMorita 23:1509648c2318 65 {
NaotoMorita 44:7d82e63b6a86 66 Matrix gyrom = gyro - gyroBias;
NaotoMorita 44:7d82e63b6a86 67 Matrix accm = acc - accBias;
NaotoMorita 23:1509648c2318 68
NaotoMorita 23:1509648c2318 69 Matrix dcm(3,3);
NaotoMorita 23:1509648c2318 70 computeDcm(dcm, qhat);
NaotoMorita 47:2467de40951f 71 Matrix a2v = -dcm*MatrixMath::Matrixcross(accm(1,1),accm(2,1),accm(3,1));
NaotoMorita 44:7d82e63b6a86 72
NaotoMorita 47:2467de40951f 73 Matrix A(nState,nState);
NaotoMorita 44:7d82e63b6a86 74 //position
NaotoMorita 47:2467de40951f 75 A(1,4) = 1.0f;
NaotoMorita 47:2467de40951f 76 A(2,5) = 1.0f;
NaotoMorita 47:2467de40951f 77 A(3,6) = 1.0f;
NaotoMorita 47:2467de40951f 78
NaotoMorita 47:2467de40951f 79 //velocity
NaotoMorita 46:15988dc41923 80 for (int i = 1; i < 4; i++){
NaotoMorita 46:15988dc41923 81 for (int j = 1; j < 4; j++){
NaotoMorita 47:2467de40951f 82 A(i+3,j+6) = a2v(i,j);
NaotoMorita 47:2467de40951f 83 A(i+3,j+9) = -dcm(i,j);
NaotoMorita 47:2467de40951f 84
NaotoMorita 23:1509648c2318 85 }
NaotoMorita 23:1509648c2318 86 }
NaotoMorita 59:03fe5e16a33c 87 A(6,16) = 1.0f;
NaotoMorita 23:1509648c2318 88
NaotoMorita 44:7d82e63b6a86 89 //angulat error
NaotoMorita 47:2467de40951f 90 A(7,8) = gyrom(3,1);
NaotoMorita 47:2467de40951f 91 A(7,9) = -gyrom(2,1);
NaotoMorita 47:2467de40951f 92 A(8,7) = -gyrom(3,1);
NaotoMorita 47:2467de40951f 93 A(8,9) = gyrom(1,1);
NaotoMorita 47:2467de40951f 94 A(9,7) = gyrom(2,1);
NaotoMorita 47:2467de40951f 95 A(9,8) = -gyrom(1,1);
NaotoMorita 47:2467de40951f 96 A(7,13) = -1.0f;
NaotoMorita 47:2467de40951f 97 A(8,14) = -1.0f;
NaotoMorita 47:2467de40951f 98 A(9,15) = -1.0f;
NaotoMorita 25:07ac5c6cd61c 99
NaotoMorita 47:2467de40951f 100
NaotoMorita 47:2467de40951f 101 Matrix Fx = MatrixMath::Eye(nState) + A * att_dt;// + (0.5f*att_dt*att_dt) * A * A;
NaotoMorita 59:03fe5e16a33c 102 for (int i = 1; i < nState+1; i++){
NaotoMorita 59:03fe5e16a33c 103 if(i>3 && i<10){
NaotoMorita 59:03fe5e16a33c 104 Q(i,i) *=att_dt;
NaotoMorita 59:03fe5e16a33c 105 }else if(i>9 && i<16){
NaotoMorita 59:03fe5e16a33c 106 Q(i,i) *= att_dt*att_dt;
NaotoMorita 59:03fe5e16a33c 107 }else if(i>16 && i<nState+1){
NaotoMorita 59:03fe5e16a33c 108 Q(i,i) *=att_dt*att_dt;
NaotoMorita 59:03fe5e16a33c 109 }else{
NaotoMorita 59:03fe5e16a33c 110 Q(i,i) = 0.0f;
NaotoMorita 59:03fe5e16a33c 111 }
NaotoMorita 59:03fe5e16a33c 112 }
NaotoMorita 44:7d82e63b6a86 113 errState = Fx * errState;
NaotoMorita 59:03fe5e16a33c 114 Phat = Fx*Phat*MatrixMath::Transpose(Fx)+Q;
NaotoMorita 25:07ac5c6cd61c 115 }
NaotoMorita 25:07ac5c6cd61c 116
NaotoMorita 61:5e5c4fe12440 117 void solaESKF::updateAcc(Matrix acc,Matrix R)
NaotoMorita 46:15988dc41923 118 {
NaotoMorita 46:15988dc41923 119 Matrix accm = acc - accBias;
NaotoMorita 56:c10f1168bd4a 120 Matrix dcm(3,3);
NaotoMorita 56:c10f1168bd4a 121 computeDcm(dcm, qhat);
NaotoMorita 56:c10f1168bd4a 122 Matrix tdcm = MatrixMath::Transpose(dcm);
NaotoMorita 46:15988dc41923 123 Matrix tdcm_g = tdcm*gravity;
NaotoMorita 59:03fe5e16a33c 124 Matrix rotgrav = MatrixMath::Matrixcross(tdcm_g(1,1),tdcm_g(2,1),tdcm_g(3,1));
NaotoMorita 46:15988dc41923 125
NaotoMorita 61:5e5c4fe12440 126 Matrix H(3,nState);
NaotoMorita 59:03fe5e16a33c 127 for (int i = 1; i < 4; i++){
NaotoMorita 59:03fe5e16a33c 128 for (int j = 1; j < 4; j++){
NaotoMorita 59:03fe5e16a33c 129 H(i,j+6) = rotgrav(i,j);
NaotoMorita 46:15988dc41923 130 }
NaotoMorita 59:03fe5e16a33c 131 H(i,16) = tdcm(i,3);
NaotoMorita 58:93ba28cf5cb3 132 }
NaotoMorita 56:c10f1168bd4a 133
NaotoMorita 46:15988dc41923 134 H(1,10) = -1.0f;
NaotoMorita 46:15988dc41923 135 H(2,11) = -1.0f;
NaotoMorita 46:15988dc41923 136 H(3,12) = -1.0f;
NaotoMorita 59:03fe5e16a33c 137
NaotoMorita 61:5e5c4fe12440 138 Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
NaotoMorita 61:5e5c4fe12440 139 Matrix zacc = -accm-tdcm*gravity;
NaotoMorita 61:5e5c4fe12440 140 Matrix z(3,1);
NaotoMorita 61:5e5c4fe12440 141 z << zacc(1,1) << zacc(2,1) << zacc(3,1);
NaotoMorita 61:5e5c4fe12440 142 errState = K * z;
NaotoMorita 61:5e5c4fe12440 143 Phat = (MatrixMath::Eye(nState)-K*H)*Phat;
NaotoMorita 61:5e5c4fe12440 144
NaotoMorita 61:5e5c4fe12440 145 fuseErr2Nominal();
NaotoMorita 61:5e5c4fe12440 146 }
NaotoMorita 61:5e5c4fe12440 147 void solaESKF::updateMag(Matrix mag,Matrix R)
NaotoMorita 61:5e5c4fe12440 148 {
NaotoMorita 62:5519d34eb6e8 149 Matrix magm = mag-magBias;
NaotoMorita 61:5e5c4fe12440 150 Matrix dcm(3,3);
NaotoMorita 61:5e5c4fe12440 151 computeDcm(dcm, qhat);
NaotoMorita 61:5e5c4fe12440 152 Matrix tdcm = MatrixMath::Transpose(dcm);
NaotoMorita 61:5e5c4fe12440 153 Matrix rotmag = -dcm*MatrixMath::Matrixcross(magm(1,1),magm(2,1),magm(3,1));
NaotoMorita 61:5e5c4fe12440 154
NaotoMorita 61:5e5c4fe12440 155 Matrix H(3,nState);
NaotoMorita 61:5e5c4fe12440 156
NaotoMorita 59:03fe5e16a33c 157 Matrix magned = dcm*magm;
NaotoMorita 59:03fe5e16a33c 158 float hx = sqrt(magned(1,1)*magned(1,1)+magned(2,1)*magned(2,1));
NaotoMorita 62:5519d34eb6e8 159 //float hz = sqrt(magned(3,1)*magned(3,1));
NaotoMorita 62:5519d34eb6e8 160 float a = sqrt(magm(1,1)*magm(1,1)+magm(2,1)*magm(2,1)+magm(3,1)*magm(3,1));
NaotoMorita 61:5e5c4fe12440 161
NaotoMorita 59:03fe5e16a33c 162 for(int j = 1; j < 4; j++){
NaotoMorita 61:5e5c4fe12440 163 H(1,j+6) = rotmag(1,j)-(rotmag(1,j)+rotmag(2,j))/hx;
NaotoMorita 61:5e5c4fe12440 164 H(2,j+6) = rotmag(2,j);
NaotoMorita 62:5519d34eb6e8 165 //H(3,j+6) = rotmag(3,j)-(rotmag(3,j))/hz;
NaotoMorita 62:5519d34eb6e8 166 H(1,j+16) = -dcm(1,j)+(dcm(1,j)+dcm(2,j))/hx;
NaotoMorita 62:5519d34eb6e8 167 H(2,j+16) = -dcm(2,j);
NaotoMorita 59:03fe5e16a33c 168 }
NaotoMorita 62:5519d34eb6e8 169 H(3,20) = 1.0f;
NaotoMorita 62:5519d34eb6e8 170 H(3,17) = magm(1,1)/a;
NaotoMorita 62:5519d34eb6e8 171 H(3,18) = magm(2,1)/a;
NaotoMorita 62:5519d34eb6e8 172 H(3,19) = magm(3,1)/a;
NaotoMorita 61:5e5c4fe12440 173
NaotoMorita 61:5e5c4fe12440 174 Matrix z(3,1);
NaotoMorita 62:5519d34eb6e8 175 z << -(magned(1,1) - hx) << -magned(2,1) <<-(magRadius-a) ;
NaotoMorita 62:5519d34eb6e8 176 Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
NaotoMorita 46:15988dc41923 177 errState = K * z;
NaotoMorita 50:dadad0567349 178 Phat = (MatrixMath::Eye(nState)-K*H)*Phat;
NaotoMorita 58:93ba28cf5cb3 179
NaotoMorita 58:93ba28cf5cb3 180 fuseErr2Nominal();
NaotoMorita 58:93ba28cf5cb3 181 }
NaotoMorita 58:93ba28cf5cb3 182
NaotoMorita 58:93ba28cf5cb3 183
NaotoMorita 59:03fe5e16a33c 184 void solaESKF::updateGPS(Matrix posgps,float palt,Matrix velgps,Matrix R)
NaotoMorita 58:93ba28cf5cb3 185 {
NaotoMorita 59:03fe5e16a33c 186 Matrix H(5,nState);
NaotoMorita 46:15988dc41923 187 H(1,1) = 1.0f;
NaotoMorita 46:15988dc41923 188 H(2,2) = 1.0f;
NaotoMorita 56:c10f1168bd4a 189 H(3,3) = 1.0f;
NaotoMorita 56:c10f1168bd4a 190 H(4,4) = 1.0f;
NaotoMorita 56:c10f1168bd4a 191 H(5,5) = 1.0f;
NaotoMorita 53:8b551358a7e3 192 Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
NaotoMorita 59:03fe5e16a33c 193 Matrix z(5,1);
NaotoMorita 59:03fe5e16a33c 194 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 195 errState = K * z;
NaotoMorita 58:93ba28cf5cb3 196 Phat = (MatrixMath::Eye(nState)-K*H)*Phat;
NaotoMorita 58:93ba28cf5cb3 197 fuseErr2Nominal();
NaotoMorita 58:93ba28cf5cb3 198 }
NaotoMorita 58:93ba28cf5cb3 199
NaotoMorita 46:15988dc41923 200
NaotoMorita 44:7d82e63b6a86 201 Matrix solaESKF::computeAngles()
NaotoMorita 25:07ac5c6cd61c 202 {
NaotoMorita 25:07ac5c6cd61c 203
NaotoMorita 44:7d82e63b6a86 204 Matrix euler(3,1);
NaotoMorita 44:7d82e63b6a86 205 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 206 euler(2,1) = asinf(-2.0f * (qhat(2,1)*qhat(4,1) - qhat(1,1)*qhat(3,1)));
NaotoMorita 44:7d82e63b6a86 207 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 208 return euler;
NaotoMorita 19:3fae66745363 209 }
NaotoMorita 19:3fae66745363 210
NaotoMorita 21:d6079def0473 211
NaotoMorita 44:7d82e63b6a86 212 void solaESKF::fuseErr2Nominal()
NaotoMorita 19:3fae66745363 213 {
NaotoMorita 44:7d82e63b6a86 214 //position
NaotoMorita 44:7d82e63b6a86 215 pihat(1,1) += errState(1,1);
NaotoMorita 44:7d82e63b6a86 216 pihat(2,1) += errState(2,1);
NaotoMorita 44:7d82e63b6a86 217 pihat(3,1) += errState(3,1);
NaotoMorita 44:7d82e63b6a86 218
NaotoMorita 44:7d82e63b6a86 219 //velocity
NaotoMorita 44:7d82e63b6a86 220 vihat(1,1) += errState(4,1);
NaotoMorita 44:7d82e63b6a86 221 vihat(2,1) += errState(5,1);
NaotoMorita 44:7d82e63b6a86 222 vihat(3,1) += errState(6,1);
NaotoMorita 44:7d82e63b6a86 223
NaotoMorita 44:7d82e63b6a86 224 //angle error
NaotoMorita 19:3fae66745363 225 Matrix qerr(4,1);
NaotoMorita 46:15988dc41923 226 qerr << 1.0f << 0.5f*errState(7,1) << 0.5f*errState(8,1) << 0.5f*errState(9,1);
NaotoMorita 19:3fae66745363 227 qhat = quatmultiply(qhat, qerr);
NaotoMorita 19:3fae66745363 228 float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qhat),qhat));
NaotoMorita 19:3fae66745363 229 qhat *= (1.0f/ qnorm);
NaotoMorita 23:1509648c2318 230
NaotoMorita 44:7d82e63b6a86 231 //acc bias
NaotoMorita 44:7d82e63b6a86 232 accBias(1,1) += errState(10,1);
NaotoMorita 44:7d82e63b6a86 233 accBias(2,1) += errState(11,1);
NaotoMorita 44:7d82e63b6a86 234 accBias(3,1) += errState(12,1);
NaotoMorita 44:7d82e63b6a86 235
NaotoMorita 44:7d82e63b6a86 236 //gyro bias
NaotoMorita 44:7d82e63b6a86 237 gyroBias(1,1) += errState(13,1);
NaotoMorita 44:7d82e63b6a86 238 gyroBias(2,1) += errState(14,1);
NaotoMorita 44:7d82e63b6a86 239 gyroBias(3,1) += errState(15,1);
NaotoMorita 44:7d82e63b6a86 240
NaotoMorita 44:7d82e63b6a86 241 //gravity bias
NaotoMorita 59:03fe5e16a33c 242 gravity(1,1) = 0.0f;
NaotoMorita 59:03fe5e16a33c 243 gravity(2,1) = 0.0f;
NaotoMorita 59:03fe5e16a33c 244 gravity(3,1) += errState(16,1);
NaotoMorita 47:2467de40951f 245
NaotoMorita 62:5519d34eb6e8 246 //Magnetic bias
NaotoMorita 62:5519d34eb6e8 247 magBias(1,1) += errState(17,1);
NaotoMorita 62:5519d34eb6e8 248 magBias(2,1) += errState(18,1);
NaotoMorita 62:5519d34eb6e8 249 magBias(3,1) += errState(19,1);
NaotoMorita 62:5519d34eb6e8 250 magRadius += errState(20,1);
NaotoMorita 62:5519d34eb6e8 251
NaotoMorita 56:c10f1168bd4a 252
NaotoMorita 58:93ba28cf5cb3 253 for (int i = 1; i < nState+1; i++){
NaotoMorita 47:2467de40951f 254 errState(i,1) = 0.0f;
NaotoMorita 47:2467de40951f 255 }
NaotoMorita 44:7d82e63b6a86 256
NaotoMorita 19:3fae66745363 257 }
NaotoMorita 19:3fae66745363 258
NaotoMorita 44:7d82e63b6a86 259 Matrix solaESKF::quatmultiply(Matrix p, Matrix q)
NaotoMorita 19:3fae66745363 260 {
NaotoMorita 44:7d82e63b6a86 261
NaotoMorita 19:3fae66745363 262 Matrix qout(4,1);
NaotoMorita 44:7d82e63b6a86 263 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 264 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 265 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 266 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 267 float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qout),qout));
NaotoMorita 19:3fae66745363 268 qout *= (1.0f/ qnorm);
NaotoMorita 19:3fae66745363 269 return qout;
NaotoMorita 19:3fae66745363 270 }
NaotoMorita 19:3fae66745363 271
NaotoMorita 44:7d82e63b6a86 272 void solaESKF::computeDcm(Matrix& dcm, Matrix quat)
NaotoMorita 19:3fae66745363 273 {
NaotoMorita 19:3fae66745363 274
NaotoMorita 44:7d82e63b6a86 275 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 276 dcm(1,2) = 2.0f*(quat(2,1)*quat(3,1) - quat(1,1)*quat(4,1));
NaotoMorita 44:7d82e63b6a86 277 dcm(1,3) = 2.0f*(quat(2,1)*quat(4,1) + quat(1,1)*quat(3,1));
NaotoMorita 44:7d82e63b6a86 278 dcm(2,1) = 2.0f*(quat(2,1)*quat(3,1) + quat(1,1)*quat(4,1));
NaotoMorita 44:7d82e63b6a86 279 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 280 dcm(2,3) = 2.0f*(quat(3,1)*quat(4,1) - quat(1,1)*quat(2,1));
NaotoMorita 44:7d82e63b6a86 281 dcm(3,1) = 2.0f*(quat(2,1)*quat(4,1) - quat(1,1)*quat(3,1));
NaotoMorita 44:7d82e63b6a86 282 dcm(3,2) = 2.0f*(quat(3,1)*quat(4,1) + quat(1,1)*quat(2,1));
NaotoMorita 44:7d82e63b6a86 283 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 284
NaotoMorita 19:3fae66745363 285 }
NaotoMorita 19:3fae66745363 286
NaotoMorita 44:7d82e63b6a86 287 void solaESKF::setQhat(float ex,float ey,float ez)
NaotoMorita 44:7d82e63b6a86 288 {
NaotoMorita 44:7d82e63b6a86 289 float cos_z_2 = cosf(0.5f*ez);
NaotoMorita 44:7d82e63b6a86 290 float cos_y_2 = cosf(0.5f*ey);
NaotoMorita 44:7d82e63b6a86 291 float cos_x_2 = cosf(0.5f*ex);
NaotoMorita 19:3fae66745363 292
NaotoMorita 44:7d82e63b6a86 293 float sin_z_2 = sinf(0.5f*ez);
NaotoMorita 44:7d82e63b6a86 294 float sin_y_2 = sinf(0.5f*ey);
NaotoMorita 44:7d82e63b6a86 295 float sin_x_2 = sinf(0.5f*ex);
NaotoMorita 19:3fae66745363 296
NaotoMorita 19:3fae66745363 297 // and now compute quaternion
NaotoMorita 19:3fae66745363 298 qhat(1,1) = cos_z_2*cos_y_2*cos_x_2 + sin_z_2*sin_y_2*sin_x_2;
NaotoMorita 19:3fae66745363 299 qhat(2,1) = cos_z_2*cos_y_2*sin_x_2 - sin_z_2*sin_y_2*cos_x_2;
NaotoMorita 19:3fae66745363 300 qhat(3,1) = cos_z_2*sin_y_2*cos_x_2 + sin_z_2*cos_y_2*sin_x_2;
NaotoMorita 19:3fae66745363 301 qhat(4,1) = sin_z_2*cos_y_2*cos_x_2 - cos_z_2*sin_y_2*sin_x_2;
NaotoMorita 19:3fae66745363 302 }
NaotoMorita 19:3fae66745363 303
NaotoMorita 47:2467de40951f 304
NaotoMorita 47:2467de40951f 305
NaotoMorita 44:7d82e63b6a86 306 void solaESKF::setGravity(float gx,float gy,float gz)
NaotoMorita 23:1509648c2318 307 {
NaotoMorita 44:7d82e63b6a86 308 gravity(1,1) = gx;
NaotoMorita 44:7d82e63b6a86 309 gravity(2,1) = gy;
NaotoMorita 44:7d82e63b6a86 310 gravity(3,1) = gz;
NaotoMorita 23:1509648c2318 311 }
NaotoMorita 62:5519d34eb6e8 312
NaotoMorita 47:2467de40951f 313 Matrix solaESKF::getPihat()
NaotoMorita 47:2467de40951f 314 {
NaotoMorita 47:2467de40951f 315 return pihat;
NaotoMorita 47:2467de40951f 316 }
NaotoMorita 47:2467de40951f 317 Matrix solaESKF::getVihat()
NaotoMorita 47:2467de40951f 318 {
NaotoMorita 47:2467de40951f 319 return vihat;
NaotoMorita 47:2467de40951f 320 }
NaotoMorita 47:2467de40951f 321 Matrix solaESKF::getQhat()
NaotoMorita 47:2467de40951f 322 {
NaotoMorita 47:2467de40951f 323 return qhat;
NaotoMorita 47:2467de40951f 324 }
NaotoMorita 47:2467de40951f 325 Matrix solaESKF::getAccBias()
NaotoMorita 47:2467de40951f 326 {
NaotoMorita 47:2467de40951f 327 return accBias;
NaotoMorita 47:2467de40951f 328 }
NaotoMorita 47:2467de40951f 329 Matrix solaESKF::getGyroBias()
NaotoMorita 47:2467de40951f 330 {
NaotoMorita 47:2467de40951f 331 return gyroBias;
NaotoMorita 47:2467de40951f 332 }
NaotoMorita 47:2467de40951f 333 Matrix solaESKF::getGravity()
NaotoMorita 47:2467de40951f 334 {
NaotoMorita 47:2467de40951f 335 return gravity;
NaotoMorita 47:2467de40951f 336 }
NaotoMorita 62:5519d34eb6e8 337 Matrix solaESKF::getMagBias()
NaotoMorita 62:5519d34eb6e8 338 {
NaotoMorita 62:5519d34eb6e8 339 return magBias;
NaotoMorita 62:5519d34eb6e8 340 }
NaotoMorita 62:5519d34eb6e8 341 float solaESKF::getMagRadius()
NaotoMorita 62:5519d34eb6e8 342 {
NaotoMorita 62:5519d34eb6e8 343 return magRadius;
NaotoMorita 62:5519d34eb6e8 344 }
NaotoMorita 47:2467de40951f 345 Matrix solaESKF::getErrState()
NaotoMorita 47:2467de40951f 346 {
NaotoMorita 47:2467de40951f 347 return errState;
NaotoMorita 47:2467de40951f 348 }
NaotoMorita 23:1509648c2318 349
NaotoMorita 47:2467de40951f 350 void solaESKF::setPhatPosition(float val)
NaotoMorita 47:2467de40951f 351 {
NaotoMorita 47:2467de40951f 352 setBlockDiag(Phat,val,1,3);
NaotoMorita 47:2467de40951f 353 }
NaotoMorita 47:2467de40951f 354 void solaESKF::setPhatVelocity(float val)
NaotoMorita 47:2467de40951f 355 {
NaotoMorita 47:2467de40951f 356 setBlockDiag(Phat,val,4,6);
NaotoMorita 47:2467de40951f 357 }
NaotoMorita 47:2467de40951f 358 void solaESKF::setPhatAngleError(float val)
NaotoMorita 47:2467de40951f 359 {
NaotoMorita 47:2467de40951f 360 setBlockDiag(Phat,val,7,9);
NaotoMorita 47:2467de40951f 361 }
NaotoMorita 47:2467de40951f 362 void solaESKF::setPhatAccBias(float val)
NaotoMorita 47:2467de40951f 363 {
NaotoMorita 47:2467de40951f 364 setBlockDiag(Phat,val,10,12);
NaotoMorita 47:2467de40951f 365 }
NaotoMorita 47:2467de40951f 366 void solaESKF::setPhatGyroBias(float val)
NaotoMorita 47:2467de40951f 367 {
NaotoMorita 47:2467de40951f 368 setBlockDiag(Phat,val,13,15);
NaotoMorita 47:2467de40951f 369 }
NaotoMorita 47:2467de40951f 370 void solaESKF::setPhatGravity(float val)
NaotoMorita 47:2467de40951f 371 {
NaotoMorita 59:03fe5e16a33c 372 setBlockDiag(Phat,val,16,16);
NaotoMorita 47:2467de40951f 373 }
NaotoMorita 62:5519d34eb6e8 374 void solaESKF::setPhatMagBias(float val)
NaotoMorita 62:5519d34eb6e8 375 {
NaotoMorita 62:5519d34eb6e8 376 setBlockDiag(Phat,val,17,19);
NaotoMorita 62:5519d34eb6e8 377 }
NaotoMorita 62:5519d34eb6e8 378 void solaESKF::setPhatMagRadius(float val)
NaotoMorita 62:5519d34eb6e8 379 {
NaotoMorita 62:5519d34eb6e8 380 setBlockDiag(Phat,val,20,20);
NaotoMorita 62:5519d34eb6e8 381 }
NaotoMorita 47:2467de40951f 382
NaotoMorita 47:2467de40951f 383
NaotoMorita 47:2467de40951f 384 void solaESKF::setQVelocity(float val)
NaotoMorita 47:2467de40951f 385 {
NaotoMorita 47:2467de40951f 386 setBlockDiag(Q,val,4,6);
NaotoMorita 47:2467de40951f 387 }
NaotoMorita 47:2467de40951f 388 void solaESKF::setQAngleError(float val)
NaotoMorita 47:2467de40951f 389 {
NaotoMorita 47:2467de40951f 390 setBlockDiag(Q,val,7,9);
NaotoMorita 47:2467de40951f 391 }
NaotoMorita 47:2467de40951f 392 void solaESKF::setQAccBias(float val)
NaotoMorita 47:2467de40951f 393 {
NaotoMorita 47:2467de40951f 394 setBlockDiag(Q,val,10,12);
NaotoMorita 47:2467de40951f 395 }
NaotoMorita 47:2467de40951f 396 void solaESKF::setQGyroBias(float val)
NaotoMorita 47:2467de40951f 397 {
NaotoMorita 47:2467de40951f 398 setBlockDiag(Q,val,13,15);
NaotoMorita 47:2467de40951f 399 }
NaotoMorita 62:5519d34eb6e8 400 void solaESKF::setQMagBias(float val)
NaotoMorita 62:5519d34eb6e8 401 {
NaotoMorita 62:5519d34eb6e8 402 setBlockDiag(Q,val,17,19);
NaotoMorita 62:5519d34eb6e8 403 }
NaotoMorita 62:5519d34eb6e8 404 void solaESKF::setQMagRadius(float val)
NaotoMorita 62:5519d34eb6e8 405 {
NaotoMorita 62:5519d34eb6e8 406 setBlockDiag(Q,val,20,20);
NaotoMorita 62:5519d34eb6e8 407 }
NaotoMorita 19:3fae66745363 408
NaotoMorita 19:3fae66745363 409
NaotoMorita 44:7d82e63b6a86 410 void solaESKF::setDiag(Matrix& mat, float val){
NaotoMorita 44:7d82e63b6a86 411 for (int i = 1; i < mat.getCols()+1; i++){
NaotoMorita 44:7d82e63b6a86 412 mat(i,i) = val;
NaotoMorita 22:7d84b8bc20b4 413 }
NaotoMorita 22:7d84b8bc20b4 414 }
NaotoMorita 22:7d84b8bc20b4 415
NaotoMorita 45:df4618814803 416 void solaESKF::setBlockDiag(Matrix& mat, float val,int startIndex, int endIndex){
NaotoMorita 44:7d82e63b6a86 417 for (int i = startIndex; i < endIndex+1; i++){
NaotoMorita 44:7d82e63b6a86 418 mat(i,i) = val;
NaotoMorita 22:7d84b8bc20b4 419 }
NaotoMorita 22:7d84b8bc20b4 420 }
osaka 51:a5af3b280d23 421
NaotoMorita 58:93ba28cf5cb3 422 void solaESKF::setPihat(float pi_x, float pi_y)
osaka 51:a5af3b280d23 423 {
NaotoMorita 56:c10f1168bd4a 424 pihat(1,1) = pi_x;
NaotoMorita 56:c10f1168bd4a 425 pihat(2,1) = pi_y;
NaotoMorita 58:93ba28cf5cb3 426 //pihat(3,1) = pi_z;
NaotoMorita 56:c10f1168bd4a 427 }
NaotoMorita 56:c10f1168bd4a 428 /*
NaotoMorita 56:c10f1168bd4a 429 void solaESKF::updateAccConstraints(Matrix acc,float palt,Matrix R)
NaotoMorita 56:c10f1168bd4a 430 {
NaotoMorita 56:c10f1168bd4a 431 Matrix accm = acc - accBias;
NaotoMorita 56:c10f1168bd4a 432 Matrix tdcm(3,3);
NaotoMorita 56:c10f1168bd4a 433 computeDcm(tdcm, qhat);
NaotoMorita 56:c10f1168bd4a 434 tdcm = MatrixMath::Transpose(tdcm);
NaotoMorita 56:c10f1168bd4a 435 Matrix tdcm_g = tdcm*gravity;
NaotoMorita 56:c10f1168bd4a 436 Matrix a2v = MatrixMath::Matrixcross(tdcm_g(1,1),tdcm_g(2,1),tdcm_g(3,1));
NaotoMorita 56:c10f1168bd4a 437
NaotoMorita 56:c10f1168bd4a 438 Matrix H(4,nState);
NaotoMorita 56:c10f1168bd4a 439 for (int i = 1; i < 4; i++){
NaotoMorita 56:c10f1168bd4a 440 for (int j = 1; j < 4; j++){
NaotoMorita 56:c10f1168bd4a 441 H(i,j+6) = a2v(i,j);
NaotoMorita 56:c10f1168bd4a 442 H(i,j+15) = tdcm(i,j);
NaotoMorita 56:c10f1168bd4a 443 }
NaotoMorita 56:c10f1168bd4a 444 }
NaotoMorita 56:c10f1168bd4a 445 H(1,10) = -1.0f;
NaotoMorita 56:c10f1168bd4a 446 H(2,11) = -1.0f;
NaotoMorita 56:c10f1168bd4a 447 H(3,12) = -1.0f;
NaotoMorita 56:c10f1168bd4a 448 H(4,3) = 1.0f;
NaotoMorita 56:c10f1168bd4a 449
NaotoMorita 56:c10f1168bd4a 450 //Matrix A = H*Phat*MatrixMath::Transpose(H)+R;
NaotoMorita 56:c10f1168bd4a 451 //Matrix K = (Phat*MatrixMath::Transpose(H))*(MatrixMath::Inv(MatrixMath::Transpose(A)*A+10.0f*MatrixMath::Eye(3))*MatrixMath::Transpose(A));
NaotoMorita 56:c10f1168bd4a 452 Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
NaotoMorita 56:c10f1168bd4a 453 Matrix zacc = -accm-tdcm*gravity;
NaotoMorita 56:c10f1168bd4a 454 Matrix z(4,1);
NaotoMorita 56:c10f1168bd4a 455 z << zacc(1,1) << zacc(2,1) << zacc(3,1) << palt - pihat(3,1);
NaotoMorita 56:c10f1168bd4a 456 errState = K * z;
NaotoMorita 56:c10f1168bd4a 457 Phat = (MatrixMath::Eye(nState)-K*H)*Phat;
NaotoMorita 56:c10f1168bd4a 458 //Phat = Phat-K*(H*Phat*MatrixMath::Transpose(H)+R)*MatrixMath::Transpose(K);
NaotoMorita 56:c10f1168bd4a 459 //Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*R*MatrixMath::Transpose(K);
NaotoMorita 56:c10f1168bd4a 460 fuseErr2Nominal();
NaotoMorita 56:c10f1168bd4a 461 }
NaotoMorita 56:c10f1168bd4a 462
NaotoMorita 56:c10f1168bd4a 463 void solaESKF::updateGyroConstraints(Matrix gyro,Matrix R)
NaotoMorita 56:c10f1168bd4a 464 {
NaotoMorita 56:c10f1168bd4a 465 Matrix gyrom = gyro - gyroBias;
NaotoMorita 56:c10f1168bd4a 466 Matrix dcm(3,3);
NaotoMorita 56:c10f1168bd4a 467 computeDcm(dcm, qhat);
NaotoMorita 56:c10f1168bd4a 468 Matrix a2v = dcm*MatrixMath::Matrixcross(gyrom(1,1),gyrom(2,1),gyrom(3,1));
NaotoMorita 56:c10f1168bd4a 469
NaotoMorita 56:c10f1168bd4a 470 Matrix H(2,nState);
NaotoMorita 56:c10f1168bd4a 471 for (int i = 1; i < 3; i++){
NaotoMorita 56:c10f1168bd4a 472 for (int j = 1; j < 4; j++){
NaotoMorita 56:c10f1168bd4a 473 H(i,j+6) = a2v(i,j);
NaotoMorita 56:c10f1168bd4a 474 H(i,j+12) = -dcm(i,j);
NaotoMorita 56:c10f1168bd4a 475 }
NaotoMorita 56:c10f1168bd4a 476 }
NaotoMorita 56:c10f1168bd4a 477 //Matrix A = H*Phat*MatrixMath::Transpose(H)+R;
NaotoMorita 56:c10f1168bd4a 478 //Matrix K = (Phat*MatrixMath::Transpose(H))*(MatrixMath::Inv(MatrixMath::Transpose(A)*A+10.0f*MatrixMath::Eye(2))*MatrixMath::Transpose(A));
NaotoMorita 56:c10f1168bd4a 479 Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
NaotoMorita 56:c10f1168bd4a 480
NaotoMorita 56:c10f1168bd4a 481 Matrix z3 = -dcm*gyrom;
NaotoMorita 56:c10f1168bd4a 482 Matrix z(2,1);
NaotoMorita 56:c10f1168bd4a 483 z << z3(1,1) << z3(2,1);
NaotoMorita 56:c10f1168bd4a 484 errState = K * z;
NaotoMorita 56:c10f1168bd4a 485 //Phat = (MatrixMath::Eye(nState)-K*H)*Phat;
NaotoMorita 56:c10f1168bd4a 486 //Phat = Phat-K*(H*Phat*MatrixMath::Transpose(H)+R)*MatrixMath::Transpose(K);
NaotoMorita 56:c10f1168bd4a 487 Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*R*MatrixMath::Transpose(K);
NaotoMorita 56:c10f1168bd4a 488 fuseErr2Nominal();
NaotoMorita 56:c10f1168bd4a 489 }
NaotoMorita 56:c10f1168bd4a 490 void solaESKF::updateMag(Matrix mag, Matrix R)
NaotoMorita 56:c10f1168bd4a 491 {
NaotoMorita 56:c10f1168bd4a 492 Matrix dcm(3,3);
NaotoMorita 56:c10f1168bd4a 493 computeDcm(dcm, qhat);
NaotoMorita 56:c10f1168bd4a 494 Matrix a2v = -dcm*MatrixMath::Matrixcross(mag(1,1),mag(2,1),mag(3,1));
NaotoMorita 56:c10f1168bd4a 495
NaotoMorita 56:c10f1168bd4a 496 Matrix H(2,nState);
NaotoMorita 56:c10f1168bd4a 497 for (int i = 1; i < 3; i++){
NaotoMorita 56:c10f1168bd4a 498 for (int j = 1; j < 4; j++){
NaotoMorita 56:c10f1168bd4a 499 H(i,j+6) = a2v(i,j);
NaotoMorita 56:c10f1168bd4a 500 }
NaotoMorita 56:c10f1168bd4a 501 }
NaotoMorita 56:c10f1168bd4a 502 H(1,19) = 1.0f;
NaotoMorita 56:c10f1168bd4a 503 //H(3,20) = 1.0f;
NaotoMorita 56:c10f1168bd4a 504
NaotoMorita 56:c10f1168bd4a 505 //Matrix A = H*Phat*MatrixMath::Transpose(H)+R;
NaotoMorita 56:c10f1168bd4a 506 //Matrix K = (Phat*MatrixMath::Transpose(H))*(MatrixMath::Inv(MatrixMath::Transpose(A)*A+10.0f*MatrixMath::Eye(3))*MatrixMath::Transpose(A));
NaotoMorita 56:c10f1168bd4a 507 Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
NaotoMorita 56:c10f1168bd4a 508 Matrix zmag = -dcm*mag-magField;
NaotoMorita 56:c10f1168bd4a 509 Matrix z(2,1);
NaotoMorita 56:c10f1168bd4a 510 z << zmag(1,1) << zmag(2,1);
NaotoMorita 56:c10f1168bd4a 511 errState = K * z;
NaotoMorita 56:c10f1168bd4a 512 Phat = (MatrixMath::Eye(nState)-K*H)*Phat;
NaotoMorita 56:c10f1168bd4a 513 //Phat = Phat-K*(H*Phat*MatrixMath::Transpose(H)+R)*MatrixMath::Transpose(K);
NaotoMorita 56:c10f1168bd4a 514 //Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*R*MatrixMath::Transpose(K);
NaotoMorita 56:c10f1168bd4a 515 fuseErr2Nominal();
NaotoMorita 56:c10f1168bd4a 516 }
NaotoMorita 59:03fe5e16a33c 517 void solaESKF::updateMag(Matrix mag,float palt, Matrix R)
NaotoMorita 59:03fe5e16a33c 518 {
NaotoMorita 59:03fe5e16a33c 519 Matrix dcm(3,3);
NaotoMorita 59:03fe5e16a33c 520 computeDcm(dcm, qhat);
NaotoMorita 59:03fe5e16a33c 521 Matrix a2v = -dcm*MatrixMath::Matrixcross(mag(1,1),mag(2,1),mag(3,1));
NaotoMorita 59:03fe5e16a33c 522
NaotoMorita 59:03fe5e16a33c 523 Matrix H(3,nState);
NaotoMorita 59:03fe5e16a33c 524 for (int i = 1; i < 3; i++){
NaotoMorita 59:03fe5e16a33c 525 for (int j = 1; j < 4; j++){
NaotoMorita 59:03fe5e16a33c 526 H(i,j+6) = a2v(i,j);
NaotoMorita 59:03fe5e16a33c 527 }
NaotoMorita 59:03fe5e16a33c 528 }
NaotoMorita 59:03fe5e16a33c 529 H(1,19) = 1.0f;
NaotoMorita 59:03fe5e16a33c 530 H(3,3) = 1.0f;
NaotoMorita 59:03fe5e16a33c 531 //Matrix A = H*Phat*MatrixMath::Transpose(H)+R;
NaotoMorita 59:03fe5e16a33c 532 //Matrix K = (Phat*MatrixMath::Transpose(H))*(MatrixMath::Inv(MatrixMath::Transpose(A)*A+10.0f*MatrixMath::Eye(3))*MatrixMath::Transpose(A));
NaotoMorita 59:03fe5e16a33c 533 Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
NaotoMorita 59:03fe5e16a33c 534 Matrix zmag = -dcm*mag-magField;
NaotoMorita 59:03fe5e16a33c 535 Matrix z(3,1);
NaotoMorita 59:03fe5e16a33c 536 z << zmag(1,1) << zmag(2,1) << palt - pihat(3,1);
NaotoMorita 59:03fe5e16a33c 537 errState = K * z;
NaotoMorita 59:03fe5e16a33c 538 Phat = (MatrixMath::Eye(nState)-K*H)*Phat;
NaotoMorita 59:03fe5e16a33c 539 //Phat = Phat-K*(H*Phat*MatrixMath::Transpose(H)+R)*MatrixMath::Transpose(K);
NaotoMorita 59:03fe5e16a33c 540 //Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*R*MatrixMath::Transpose(K);
NaotoMorita 59:03fe5e16a33c 541
NaotoMorita 59:03fe5e16a33c 542 fuseErr2Nominal();
NaotoMorita 59:03fe5e16a33c 543 }
NaotoMorita 59:03fe5e16a33c 544 void solaESKF::updateGPSVelocity(Matrix velgps,Matrix mag,Matrix R)
NaotoMorita 59:03fe5e16a33c 545 {
NaotoMorita 59:03fe5e16a33c 546
NaotoMorita 59:03fe5e16a33c 547 Matrix dcm(3,3);
NaotoMorita 59:03fe5e16a33c 548 computeDcm(dcm, qhat);
NaotoMorita 59:03fe5e16a33c 549 Matrix a2v = -dcm*MatrixMath::Matrixcross(mag(1,1),mag(2,1),mag(3,1));
NaotoMorita 59:03fe5e16a33c 550
NaotoMorita 59:03fe5e16a33c 551
NaotoMorita 59:03fe5e16a33c 552 Matrix H(3,nState);
NaotoMorita 59:03fe5e16a33c 553 H(1,4) = 1.0f;
NaotoMorita 59:03fe5e16a33c 554 H(2,5) = 1.0f;
NaotoMorita 59:03fe5e16a33c 555
NaotoMorita 59:03fe5e16a33c 556 for (int j = 1; j < 4; j++){
NaotoMorita 59:03fe5e16a33c 557 H(3,j+6) = a2v(2,j);
NaotoMorita 59:03fe5e16a33c 558 }
NaotoMorita 59:03fe5e16a33c 559 //H(3,19) = 1.0f;
NaotoMorita 59:03fe5e16a33c 560
NaotoMorita 59:03fe5e16a33c 561 Matrix zmag = -dcm*mag-magField;
NaotoMorita 59:03fe5e16a33c 562
NaotoMorita 59:03fe5e16a33c 563 //Matrix A = H*Phat*MatrixMath::Transpose(H)+R;
NaotoMorita 59:03fe5e16a33c 564 //Matrix K = (Phat*MatrixMath::Transpose(H))*(MatrixMath::Inv(MatrixMath::Transpose(A)*A+10.0f*MatrixMath::Eye(3))*MatrixMath::Transpose(A));
NaotoMorita 59:03fe5e16a33c 565 Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
NaotoMorita 59:03fe5e16a33c 566 Matrix z(3,1);
NaotoMorita 59:03fe5e16a33c 567 z << velgps(1,1) - vihat(1,1) << velgps(2,1)-vihat(2,1) << zmag(2,1);
NaotoMorita 59:03fe5e16a33c 568 errState = K * z;
NaotoMorita 59:03fe5e16a33c 569 Phat = (MatrixMath::Eye(nState)-K*H)*Phat;
NaotoMorita 59:03fe5e16a33c 570 //Phat = Phat-K*(H*Phat*MatrixMath::Transpose(H)+R)*MatrixMath::Transpose(K);
NaotoMorita 59:03fe5e16a33c 571 //Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*R*MatrixMath::Transpose(K);
NaotoMorita 59:03fe5e16a33c 572 fuseErr2Nominal();
NaotoMorita 59:03fe5e16a33c 573 }
NaotoMorita 56:c10f1168bd4a 574
NaotoMorita 59:03fe5e16a33c 575 void solaESKF::updateGPSPosition(Matrix posgps,float palt,Matrix R)
NaotoMorita 59:03fe5e16a33c 576 {
NaotoMorita 59:03fe5e16a33c 577 Matrix H(3,nState);
NaotoMorita 59:03fe5e16a33c 578 H(1,1) = 1.0f;
NaotoMorita 59:03fe5e16a33c 579 H(2,2) = 1.0f;
NaotoMorita 59:03fe5e16a33c 580 H(3,3) = 1.0f;
NaotoMorita 59:03fe5e16a33c 581
NaotoMorita 59:03fe5e16a33c 582 //Matrix A = H*Phat*MatrixMath::Transpose(H)+R;
NaotoMorita 59:03fe5e16a33c 583 //Matrix K = (Phat*MatrixMath::Transpose(H))*(MatrixMath::Inv(MatrixMath::Transpose(A)*A+1000.0f*MatrixMath::Eye(3))*MatrixMath::Transpose(A));
NaotoMorita 59:03fe5e16a33c 584 Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
NaotoMorita 59:03fe5e16a33c 585 Matrix z(3,1);
NaotoMorita 59:03fe5e16a33c 586 z << posgps(1,1) - pihat(1,1) << posgps(2,1)-pihat(2,1) << palt - pihat(3,1);
NaotoMorita 59:03fe5e16a33c 587 errState = K * z;
NaotoMorita 59:03fe5e16a33c 588 Phat = (MatrixMath::Eye(nState)-K*H)*Phat;
NaotoMorita 59:03fe5e16a33c 589 //Phat = Phat-K*(H*Phat*MatrixMath::Transpose(H)+R)*MatrixMath::Transpose(K);
NaotoMorita 59:03fe5e16a33c 590 //Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*R*MatrixMath::Transpose(K);
NaotoMorita 59:03fe5e16a33c 591 fuseErr2Nominal();
NaotoMorita 59:03fe5e16a33c 592 }
NaotoMorita 61:5e5c4fe12440 593
NaotoMorita 61:5e5c4fe12440 594 void solaESKF::updateImuConstraints(Matrix acc,Matrix mag,Matrix R)
NaotoMorita 61:5e5c4fe12440 595 {
NaotoMorita 61:5e5c4fe12440 596 Matrix accm = acc - accBias;
NaotoMorita 61:5e5c4fe12440 597 Matrix magm = mag - magBias;
NaotoMorita 61:5e5c4fe12440 598 Matrix dcm(3,3);
NaotoMorita 61:5e5c4fe12440 599 computeDcm(dcm, qhat);
NaotoMorita 61:5e5c4fe12440 600 Matrix tdcm = MatrixMath::Transpose(dcm);
NaotoMorita 61:5e5c4fe12440 601 Matrix tdcm_g = tdcm*gravity;
NaotoMorita 61:5e5c4fe12440 602 Matrix rotgrav = MatrixMath::Matrixcross(tdcm_g(1,1),tdcm_g(2,1),tdcm_g(3,1));
NaotoMorita 61:5e5c4fe12440 603 Matrix rotmag = -dcm*MatrixMath::Matrixcross(magm(1,1),magm(2,1),magm(3,1));
NaotoMorita 61:5e5c4fe12440 604
NaotoMorita 61:5e5c4fe12440 605 Matrix H(5,nState);
NaotoMorita 61:5e5c4fe12440 606 for (int i = 1; i < 4; i++){
NaotoMorita 61:5e5c4fe12440 607 for (int j = 1; j < 4; j++){
NaotoMorita 61:5e5c4fe12440 608 H(i,j+6) = rotgrav(i,j);
NaotoMorita 61:5e5c4fe12440 609 }
NaotoMorita 61:5e5c4fe12440 610 H(i,16) = tdcm(i,3);
NaotoMorita 61:5e5c4fe12440 611 }
NaotoMorita 61:5e5c4fe12440 612
NaotoMorita 61:5e5c4fe12440 613 H(1,10) = -1.0f;
NaotoMorita 61:5e5c4fe12440 614 H(2,11) = -1.0f;
NaotoMorita 61:5e5c4fe12440 615 H(3,12) = -1.0f;
NaotoMorita 61:5e5c4fe12440 616
NaotoMorita 61:5e5c4fe12440 617 Matrix magned = dcm*magm;
NaotoMorita 61:5e5c4fe12440 618 float hx = sqrt(magned(1,1)*magned(1,1)+magned(2,1)*magned(2,1));
NaotoMorita 61:5e5c4fe12440 619
NaotoMorita 61:5e5c4fe12440 620 for(int j = 1; j < 4; j++){
NaotoMorita 61:5e5c4fe12440 621 H(4,j+6) = rotmag(1,j)-(rotmag(1,j)+rotmag(2,j))/hx;
NaotoMorita 61:5e5c4fe12440 622 H(4,j+16) = -dcm(1,j)+(dcm(1,j)+dcm(2,j))/hx;
NaotoMorita 61:5e5c4fe12440 623 H(5,j+6) = rotmag(2,j);
NaotoMorita 61:5e5c4fe12440 624 H(5,j+16) = -dcm(2,j);
NaotoMorita 61:5e5c4fe12440 625 }
NaotoMorita 61:5e5c4fe12440 626
NaotoMorita 61:5e5c4fe12440 627 Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
NaotoMorita 61:5e5c4fe12440 628 Matrix zacc = -accm-tdcm*gravity;
NaotoMorita 61:5e5c4fe12440 629 Matrix zmag = dcm*magm;
NaotoMorita 61:5e5c4fe12440 630 Matrix z(5,1);
NaotoMorita 61:5e5c4fe12440 631 z << zacc(1,1) << zacc(2,1) << zacc(3,1) << -(zmag(1,1) - hx) << -zmag(2,1);
NaotoMorita 61:5e5c4fe12440 632 twelite.printf("%f %f\r\n",hx,(zmag(1,1) - hx));
NaotoMorita 61:5e5c4fe12440 633 errState = K * z;
NaotoMorita 61:5e5c4fe12440 634 Phat = (MatrixMath::Eye(nState)-K*H)*Phat;
NaotoMorita 61:5e5c4fe12440 635
NaotoMorita 61:5e5c4fe12440 636 fuseErr2Nominal();
NaotoMorita 61:5e5c4fe12440 637 }
NaotoMorita 56:c10f1168bd4a 638 */