HAPSRG / solaESKF_Eigen

Eigen

Dependencies:   Eigen

Dependents:   optWingforHAPS_Eigen hexaTest_Eigen

solaESKF.cpp@56:c10f1168bd4a, 2021-11-10 (annotated)

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