HAPSRG / solaESKF_Eigen

Eigen

Dependencies:   Eigen

Dependents:   optWingforHAPS_Eigen hexaTest_Eigen

solaESKF.cpp@51:a5af3b280d23, 2021-11-03 (annotated)

Committer:
osaka
Date:
Wed Nov 03 09:27:58 2021 +0000
Revision:
51:a5af3b280d23
Parent:
50:dadad0567349
Child:
52:ab42c0497088
setPihat() added

Who changed what in which revision?

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