HAPSRG / solaESKF_Eigen

Eigen

Dependencies:   Eigen

Dependents:   optWingforHAPS_Eigen hexaTest_Eigen

solaESKF.cpp@58:93ba28cf5cb3, 2021-11-12 (annotated)

Committer:
NaotoMorita
Date:
Fri Nov 12 09:03:10 2021 +0000
Revision:
58:93ba28cf5cb3
Parent:
56:c10f1168bd4a
Child:
59:03fe5e16a33c
gps update

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