HAPSRG / solaESKF_Eigen

Eigen

Dependencies:   Eigen

Dependents:   optWingforHAPS_Eigen hexaTest_Eigen

solaESKF.cpp@69:2b2815603e5a, 2021-11-19 (annotated)

Committer:
NaotoMorita
Date:
Fri Nov 19 05:44:14 2021 +0000
Revision:
69:2b2815603e5a
Parent:
68:264a7e0e4a29 
hatena

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