HAPSRG / solaESKF_Eigen

Eigen

Dependencies:   Eigen

Dependents:   optWingforHAPS_Eigen hexaTest_Eigen

solaESKF.cpp@70:d12e46fdc2f0, 2021-11-19 (annotated)

Committer:
NaotoMorita
Date:
Fri Nov 19 08:21:44 2021 +0000
Revision:
70:d12e46fdc2f0
Parent:
68:264a7e0e4a29
Child:
71:56c32be982b8
heading fuse

Who changed what in which revision?

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