HAPSRG / solaESKF_Eigen

Eigen

Dependencies:   Eigen

Dependents:   optWingforHAPS_Eigen hexaTest_Eigen

solaESKF.cpp@74:f5fe7fecbd3c, 2021-11-22 (annotated)

Committer:
NaotoMorita
Date:
Mon Nov 22 09:51:36 2021 +0000
Revision:
74:f5fe7fecbd3c
Parent:
73:5770a0d470c0
Child:
75:e2c825cdc511
mag fuse OK

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