HAPSRG / solaESKF_Eigen

Eigen

Dependencies:   Eigen

Dependents:   optWingforHAPS_Eigen hexaTest_Eigen

solaESKF.cpp@64:f5b6256726f3, 2021-11-17 (annotated)

Committer:
NaotoMorita
Date:
Wed Nov 17 05:11:57 2021 +0000
Revision:
64:f5b6256726f3
Parent:
61:5e5c4fe12440
Child:
67:0453896df0d6
suh mod

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