HAPSRG / solaESKF_Eigen

Eigen

Dependencies:   Eigen

Dependents:   optWingforHAPS_Eigen hexaTest_Eigen

solaESKF.cpp@60:f4b4231a8d3f, 2021-11-16 (annotated)

Committer:
NaotoMorita
Date:
Tue Nov 16 01:20:50 2021 +0000
Revision:
60:f4b4231a8d3f
Parent:
59:03fe5e16a33c
Child:
61:5e5c4fe12440
magmod

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