HAPSRG / solaESKF_Eigen

Eigen

Dependencies:   Eigen

Dependents:   optWingforHAPS_Eigen hexaTest_Eigen

solaESKF.cpp@68:264a7e0e4a29, 2021-11-18 (annotated)

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