HAPSRG / solaESKF_Eigen

Eigen

Dependencies:   Eigen

Dependents:   optWingforHAPS_Eigen hexaTest_Eigen

solaESKF.cpp@55:21611d4cf7e8, 2021-11-08 (annotated)

Committer:
NaotoMorita
Date:
Mon Nov 08 09:19:55 2021 +0000
Revision:
55:21611d4cf7e8
Parent:
54:cd514d9d4b19
Child:
56:c10f1168bd4a
mag debuging;

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