HAPSRG / solaESKF_Eigen

Eigen

Dependencies:   Eigen

Dependents:   optWingforHAPS_Eigen hexaTest_Eigen

solaESKF.cpp@80:b241c058df83, 2022-06-24 (annotated)

Committer:
NaotoMorita
Date:
Fri Jun 24 05:43:46 2022 +0000
Revision:
80:b241c058df83
Parent:
79:365ea9277167
Child:
81:230a3d2b0493
whole update

Who changed what in which revision?

UserRevisionLine numberNew contents of line
NaotoMorita 44:7d82e63b6a86 1 #include "solaESKF.hpp"
NaotoMorita 54:cd514d9d4b19 2
NaotoMorita 44:7d82e63b6a86 3 solaESKF::solaESKF()
cocorlow 78:e36a7b844fb5 4 // :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)
cocorlow 78:e36a7b844fb5 5 :errState(18,1),Phat(18,18),Q(18,18)
NaotoMorita 62:5519d34eb6e8 6 {
cocorlow 78:e36a7b844fb5 7 pihat << 0.0f, 0.0f, 0.0f;
cocorlow 78:e36a7b844fb5 8 vihat << 0.0f, 0.0f, 0.0f;
cocorlow 78:e36a7b844fb5 9 qhat << 1.0f, 0.0f, 0.0f, 0.0f;
cocorlow 78:e36a7b844fb5 10 accBias << 0.0f, 0.0f, 0.0f;
cocorlow 78:e36a7b844fb5 11 gyroBias << 0.0f, 0.0f, 0.0f;
cocorlow 78:e36a7b844fb5 12 gravity << 0.0f, 0.0, 9.8f;
NaotoMorita 62:5519d34eb6e8 13
cocorlow 78:e36a7b844fb5 14 nState = 18;
cocorlow 78:e36a7b844fb5 15 errState = VectorXf::Zero(nState);
cocorlow 78:e36a7b844fb5 16 Phat = MatrixXf::Zero(nState, nState);
cocorlow 78:e36a7b844fb5 17 Q = MatrixXf::Zero(nState, nState);
NaotoMorita 62:5519d34eb6e8 18
cocorlow 78:e36a7b844fb5 19 setBlockDiag(Phat, 0.1f, 0, 2);//position
cocorlow 78:e36a7b844fb5 20 setBlockDiag(Phat, 0.1f, 3, 5);//velocity
cocorlow 78:e36a7b844fb5 21 setBlockDiag(Phat, 0.1f, 6, 8);//angle error
cocorlow 78:e36a7b844fb5 22 setBlockDiag(Phat, 0.1f, 9, 11);//acc bias
cocorlow 78:e36a7b844fb5 23 setBlockDiag(Phat, 0.1f, 12, 14);//gyro bias
cocorlow 78:e36a7b844fb5 24 setBlockDiag(Phat, 0.00000001f, 15, 17);//gravity
cocorlow 78:e36a7b844fb5 25 setBlockDiag(Q, 0.00025f, 3, 5);//velocity
cocorlow 78:e36a7b844fb5 26 setBlockDiag(Q, 0.005f/57.0f, 6, 8);//angle error
cocorlow 78:e36a7b844fb5 27 setBlockDiag(Q, 0.001f, 9, 11);//acc bias
cocorlow 78:e36a7b844fb5 28 setBlockDiag(Q, 0.001f, 12, 14);//gyro bias//positionとgravityはQ項なし
NaotoMorita 19:3fae66745363 29 }
NaotoMorita 19:3fae66745363 30
NaotoMorita 47:2467de40951f 31
cocorlow 78:e36a7b844fb5 32 void solaESKF::updateNominal(Vector3f acc, Vector3f gyro,float att_dt)
NaotoMorita 19:3fae66745363 33 {
cocorlow 78:e36a7b844fb5 34 Vector3f gyrom = gyro - gyroBias;
cocorlow 78:e36a7b844fb5 35 Vector3f accm = acc - accBias;
NaotoMorita 44:7d82e63b6a86 36
cocorlow 78:e36a7b844fb5 37 Vector4f qint;
cocorlow 78:e36a7b844fb5 38 qint << 1.0f, 0.5f*gyrom(0)*att_dt, 0.5f*gyrom(1)*att_dt, 0.5f*gyrom(2)*att_dt;
cocorlow 78:e36a7b844fb5 39 qhat = quatmultiply(qhat, qint);
cocorlow 78:e36a7b844fb5 40 qhat.normalize();
NaotoMorita 23:1509648c2318 41
cocorlow 78:e36a7b844fb5 42 Matrix3f dcm;
NaotoMorita 23:1509648c2318 43 computeDcm(dcm, qhat);
cocorlow 78:e36a7b844fb5 44 Vector3f accned = dcm*accm + gravity;
NaotoMorita 44:7d82e63b6a86 45 vihat += accned*att_dt;
NaotoMorita 44:7d82e63b6a86 46
cocorlow 78:e36a7b844fb5 47 pihat += vihat*att_dt + 0.5f*accned*att_dt*att_dt;
NaotoMorita 19:3fae66745363 48 }
NaotoMorita 19:3fae66745363 49
cocorlow 78:e36a7b844fb5 50 void solaESKF::updateErrState(Vector3f acc, Vector3f gyro, float att_dt)
NaotoMorita 23:1509648c2318 51 {
cocorlow 78:e36a7b844fb5 52 Vector3f gyrom = gyro - gyroBias;
cocorlow 78:e36a7b844fb5 53 Vector3f accm = acc - accBias;
NaotoMorita 75:e2c825cdc511 54
cocorlow 78:e36a7b844fb5 55 Matrix3f dcm;
NaotoMorita 23:1509648c2318 56 computeDcm(dcm, qhat);
cocorlow 78:e36a7b844fb5 57 // Matrix a2v = -dcm*MatrixMath::Matrixcross(accm(1,1),accm(2,1),accm(3,1))*att_dt;
cocorlow 78:e36a7b844fb5 58 Matrix3f a2v = -dcm*solaESKF::Matrixcross(accm)*att_dt;
cocorlow 78:e36a7b844fb5 59 Matrix3f a2v2 = 0.5f*a2v*att_dt;
NaotoMorita 75:e2c825cdc511 60
NaotoMorita 80:b241c058df83 61 MatrixXf Fx = MatrixXf::Zero(nState, nState);
NaotoMorita 44:7d82e63b6a86 62 //position
cocorlow 78:e36a7b844fb5 63 Fx(0,0) = 1.0f;
NaotoMorita 75:e2c825cdc511 64 Fx(1,1) = 1.0f;
NaotoMorita 75:e2c825cdc511 65 Fx(2,2) = 1.0f;
cocorlow 78:e36a7b844fb5 66 Fx(0,3) = 1.0f*att_dt;
NaotoMorita 75:e2c825cdc511 67 Fx(1,4) = 1.0f*att_dt;
NaotoMorita 75:e2c825cdc511 68 Fx(2,5) = 1.0f*att_dt;
cocorlow 78:e36a7b844fb5 69 for (int i = 0; i < 3; i++){
cocorlow 78:e36a7b844fb5 70 for (int j = 0; j < 3; j++){
NaotoMorita 75:e2c825cdc511 71 Fx(i,j+6) = a2v2(i,j);
NaotoMorita 75:e2c825cdc511 72 Fx(i,j+9) = -0.5f*dcm(i,j)*att_dt*att_dt;
NaotoMorita 75:e2c825cdc511 73 }
NaotoMorita 75:e2c825cdc511 74 Fx(i,i+15) = 0.5f*att_dt*att_dt;
NaotoMorita 75:e2c825cdc511 75 }
NaotoMorita 75:e2c825cdc511 76
NaotoMorita 75:e2c825cdc511 77
NaotoMorita 75:e2c825cdc511 78 //velocity
cocorlow 78:e36a7b844fb5 79 Fx(3,3) = 1.0f;
NaotoMorita 75:e2c825cdc511 80 Fx(4,4) = 1.0f;
NaotoMorita 75:e2c825cdc511 81 Fx(5,5) = 1.0f;
cocorlow 78:e36a7b844fb5 82 for (int i = 0; i < 3; i++){
cocorlow 78:e36a7b844fb5 83 for (int j = 0; j < 3; j++){
NaotoMorita 75:e2c825cdc511 84 Fx(i+3,j+6) = a2v(i,j);
NaotoMorita 75:e2c825cdc511 85 Fx(i+3,j+9) = -dcm(i,j)*att_dt;
NaotoMorita 75:e2c825cdc511 86 Fx(i+3,j+12) = -a2v2(i,j);
NaotoMorita 23:1509648c2318 87 }
NaotoMorita 23:1509648c2318 88 }
cocorlow 78:e36a7b844fb5 89 Fx(3,15) = 1.0f*att_dt;
NaotoMorita 75:e2c825cdc511 90 Fx(4,16) = 1.0f*att_dt;
NaotoMorita 75:e2c825cdc511 91 Fx(5,17) = 1.0f*att_dt;
NaotoMorita 75:e2c825cdc511 92
NaotoMorita 44:7d82e63b6a86 93 //angulat error
cocorlow 78:e36a7b844fb5 94 Fx(6,6) = 1.0f;
NaotoMorita 75:e2c825cdc511 95 Fx(7,7) = 1.0f;
NaotoMorita 75:e2c825cdc511 96 Fx(8,8) = 1.0f;
cocorlow 78:e36a7b844fb5 97 Fx(6,7) = gyrom(2)*att_dt;
cocorlow 78:e36a7b844fb5 98 Fx(6,8) = -gyrom(1)*att_dt;
cocorlow 78:e36a7b844fb5 99 Fx(7,6) = -gyrom(2)*att_dt;
cocorlow 78:e36a7b844fb5 100 Fx(7,8) = gyrom(0)*att_dt;
cocorlow 78:e36a7b844fb5 101 Fx(8,6) = gyrom(1)*att_dt;
cocorlow 78:e36a7b844fb5 102 Fx(8,7) = -gyrom(0)*att_dt;
cocorlow 78:e36a7b844fb5 103 Fx(6,12) = -1.0f*att_dt;
NaotoMorita 75:e2c825cdc511 104 Fx(7,13) = -1.0f*att_dt;
NaotoMorita 75:e2c825cdc511 105 Fx(8,14) = -1.0f*att_dt;
NaotoMorita 47:2467de40951f 106
NaotoMorita 75:e2c825cdc511 107 //acc bias
cocorlow 78:e36a7b844fb5 108 Fx(9,9) = 1.0f;
NaotoMorita 75:e2c825cdc511 109 Fx(10,10) = 1.0f;
NaotoMorita 75:e2c825cdc511 110 Fx(11,11) = 1.0f;
NaotoMorita 75:e2c825cdc511 111
NaotoMorita 75:e2c825cdc511 112 //gyro bias
cocorlow 78:e36a7b844fb5 113 Fx(12,12) = 1.0f;
NaotoMorita 75:e2c825cdc511 114 Fx(13,13) = 1.0f;
NaotoMorita 75:e2c825cdc511 115 Fx(14,14) = 1.0f;
NaotoMorita 75:e2c825cdc511 116
NaotoMorita 75:e2c825cdc511 117 //gravity bias
cocorlow 78:e36a7b844fb5 118 Fx(15,15) = 1.0f;
NaotoMorita 75:e2c825cdc511 119 Fx(16,16) = 1.0f;
NaotoMorita 75:e2c825cdc511 120 Fx(17,17) = 1.0f;
NaotoMorita 75:e2c825cdc511 121
NaotoMorita 80:b241c058df83 122 //errState = Fx * errState;
cocorlow 78:e36a7b844fb5 123 Phat = Fx*Phat*Fx.transpose();
cocorlow 78:e36a7b844fb5 124 for (int i = 0; i < nState; i++){
cocorlow 78:e36a7b844fb5 125 if(i>2 && i<9){
NaotoMorita 75:e2c825cdc511 126 Phat(i,i) += Q(i,i)*att_dt;
cocorlow 78:e36a7b844fb5 127 }else if(i>8 && i<15){
NaotoMorita 75:e2c825cdc511 128 Phat(i,i) += Q(i,i)* att_dt*att_dt;
NaotoMorita 59:03fe5e16a33c 129 }
NaotoMorita 59:03fe5e16a33c 130 }
NaotoMorita 25:07ac5c6cd61c 131 }
NaotoMorita 25:07ac5c6cd61c 132
cocorlow 78:e36a7b844fb5 133 void solaESKF::updateAcc(Vector3f acc, Matrix3f R)
NaotoMorita 46:15988dc41923 134 {
cocorlow 78:e36a7b844fb5 135 Vector3f accm = acc - accBias;
cocorlow 78:e36a7b844fb5 136 Matrix3f dcm;
NaotoMorita 56:c10f1168bd4a 137 computeDcm(dcm, qhat);
cocorlow 78:e36a7b844fb5 138 Matrix3f tdcm = dcm.transpose();
cocorlow 78:e36a7b844fb5 139 Vector3f tdcm_g = tdcm*gravity;
cocorlow 78:e36a7b844fb5 140 Matrix3f rotgrav = solaESKF::Matrixcross(tdcm_g);
NaotoMorita 46:15988dc41923 141
NaotoMorita 80:b241c058df83 142 MatrixXf H = MatrixXf::Zero(3,nState);
cocorlow 78:e36a7b844fb5 143 for (int i = 0; i < 3; i++){
cocorlow 78:e36a7b844fb5 144 for (int j = 0; j < 3; j++){
NaotoMorita 59:03fe5e16a33c 145 H(i,j+6) = rotgrav(i,j);
NaotoMorita 74:f5fe7fecbd3c 146 H(i,j+15) = tdcm(i,j);
NaotoMorita 46:15988dc41923 147 }
NaotoMorita 58:93ba28cf5cb3 148 }
NaotoMorita 56:c10f1168bd4a 149
cocorlow 78:e36a7b844fb5 150 H(0,9) = -1.0f;
NaotoMorita 46:15988dc41923 151 H(1,10) = -1.0f;
NaotoMorita 46:15988dc41923 152 H(2,11) = -1.0f;
NaotoMorita 59:03fe5e16a33c 153
cocorlow 78:e36a7b844fb5 154 // Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
cocorlow 78:e36a7b844fb5 155 MatrixXf K = (Phat*H.transpose())*(H*Phat*H.transpose()+R).inverse();
cocorlow 78:e36a7b844fb5 156 Vector3f zacc = -accm-tdcm*gravity;
cocorlow 78:e36a7b844fb5 157 Vector3f z;
cocorlow 78:e36a7b844fb5 158 z = zacc;
NaotoMorita 61:5e5c4fe12440 159 errState = K * z;
cocorlow 78:e36a7b844fb5 160 Phat = (MatrixXf::Identity(nState, nState)-K*H)*Phat;
NaotoMorita 61:5e5c4fe12440 161
NaotoMorita 61:5e5c4fe12440 162 fuseErr2Nominal();
NaotoMorita 61:5e5c4fe12440 163 }
NaotoMorita 70:d12e46fdc2f0 164
cocorlow 79:365ea9277167 165 void solaESKF::updateHeading(float a, Matrix<float, 1, 1> R)
NaotoMorita 70:d12e46fdc2f0 166 {
cocorlow 78:e36a7b844fb5 167 float q0 = qhat(0);
cocorlow 78:e36a7b844fb5 168 float q1 = qhat(1);
cocorlow 78:e36a7b844fb5 169 float q2 = qhat(2);
cocorlow 78:e36a7b844fb5 170 float q3 = qhat(3);
NaotoMorita 73:5770a0d470c0 171
NaotoMorita 73:5770a0d470c0 172 bool canUseA = false;
NaotoMorita 80:b241c058df83 173 const float SA0 = 2.0f*q3;
NaotoMorita 80:b241c058df83 174 const float SA1 = 2.0f*q2;
NaotoMorita 73:5770a0d470c0 175 const float SA2 = SA0*q0 + SA1*q1;
NaotoMorita 73:5770a0d470c0 176 const float SA3 = q0*q0 + q1*q1 - q2*q2 - q3*q3;
NaotoMorita 73:5770a0d470c0 177 float SA4, SA5_inv;
NaotoMorita 73:5770a0d470c0 178 if ((SA3*SA3) > 1e-6f) {
NaotoMorita 80:b241c058df83 179 SA4 = 1.0f/(SA3*SA3);
NaotoMorita 80:b241c058df83 180 SA5_inv = SA2*SA2*SA4 + 1.0f;
cocorlow 78:e36a7b844fb5 181 canUseA = std::abs(SA5_inv) > 1e-6f;
NaotoMorita 73:5770a0d470c0 182 }
NaotoMorita 73:5770a0d470c0 183
NaotoMorita 73:5770a0d470c0 184 bool canUseB = false;
NaotoMorita 80:b241c058df83 185 const float SB0 = 2.0f*q0;
NaotoMorita 80:b241c058df83 186 const float SB1 = 2.0f*q1;
NaotoMorita 73:5770a0d470c0 187 const float SB2 = SB0*q3 + SB1*q2;
NaotoMorita 73:5770a0d470c0 188 const float SB4 = q0*q0 + q1*q1 - q2*q2 - q3*q3;
NaotoMorita 73:5770a0d470c0 189 float SB3, SB5_inv;
NaotoMorita 73:5770a0d470c0 190 if ((SB2*SB2) > 1e-6f) {
NaotoMorita 80:b241c058df83 191 SB3 = 1.0f/(SB2*SB2);
NaotoMorita 73:5770a0d470c0 192 SB5_inv = SB3*SB4*SB4 + 1;
cocorlow 78:e36a7b844fb5 193 canUseB = std::abs(SB5_inv) > 1e-6f;
NaotoMorita 73:5770a0d470c0 194 }
NaotoMorita 73:5770a0d470c0 195
NaotoMorita 80:b241c058df83 196 MatrixXf Hh = MatrixXf::Zero(1,4);
NaotoMorita 73:5770a0d470c0 197
cocorlow 78:e36a7b844fb5 198 if (canUseA && (!canUseB || std::abs(SA5_inv) >= std::abs(SB5_inv))) {
NaotoMorita 80:b241c058df83 199 const float SA5 = 1.0f/SA5_inv;
NaotoMorita 80:b241c058df83 200 const float SA6 = 1.0f/SA3;
NaotoMorita 73:5770a0d470c0 201 const float SA7 = SA2*SA4;
NaotoMorita 80:b241c058df83 202 const float SA8 = 2.0f*SA7;
NaotoMorita 80:b241c058df83 203 const float SA9 = 2.0f*SA6;
NaotoMorita 73:5770a0d470c0 204
cocorlow 78:e36a7b844fb5 205 Hh(0,0) = SA5*(SA0*SA6 - SA8*q0);
cocorlow 78:e36a7b844fb5 206 Hh(0,1) = SA5*(SA1*SA6 - SA8*q1);
cocorlow 78:e36a7b844fb5 207 Hh(0,2) = SA5*(SA1*SA7 + SA9*q1);
cocorlow 78:e36a7b844fb5 208 Hh(0,3) = SA5*(SA0*SA7 + SA9*q0);
cocorlow 78:e36a7b844fb5 209 } else if (canUseB && (!canUseA || std::abs(SB5_inv) > std::abs(SA5_inv))) {
NaotoMorita 80:b241c058df83 210 const float SB5 = 1.0f/SB5_inv;
NaotoMorita 80:b241c058df83 211 const float SB6 = 1.0f/SB2;
NaotoMorita 73:5770a0d470c0 212 const float SB7 = SB3*SB4;
NaotoMorita 80:b241c058df83 213 const float SB8 = 2.0f*SB7;
NaotoMorita 80:b241c058df83 214 const float SB9 = 2.0f*SB6;
NaotoMorita 73:5770a0d470c0 215
cocorlow 78:e36a7b844fb5 216 Hh(0,0) = -SB5*(SB0*SB6 - SB8*q3);
cocorlow 78:e36a7b844fb5 217 Hh(0,1) = -SB5*(SB1*SB6 - SB8*q2);
cocorlow 78:e36a7b844fb5 218 Hh(0,2) = -SB5*(-SB1*SB7 - SB9*q2);
cocorlow 78:e36a7b844fb5 219 Hh(0,3) = -SB5*(-SB0*SB7 - SB9*q3);
NaotoMorita 73:5770a0d470c0 220 } else {
NaotoMorita 73:5770a0d470c0 221 return;
NaotoMorita 73:5770a0d470c0 222 }
NaotoMorita 70:d12e46fdc2f0 223
NaotoMorita 80:b241c058df83 224 MatrixXf Hdq = MatrixXf::Zero(4,3);
cocorlow 78:e36a7b844fb5 225 Hdq << -0.5f*q1, -0.5f*q2, -0.5f*q3,
cocorlow 78:e36a7b844fb5 226 0.5f*q0, -0.5f*q3, 0.5f*q2,
cocorlow 78:e36a7b844fb5 227 0.5f*q3, 0.5f*q0, -0.5f*q1,
cocorlow 78:e36a7b844fb5 228 -0.5f*q2, 0.5f*q1, 0.5f*q0;
NaotoMorita 70:d12e46fdc2f0 229
NaotoMorita 80:b241c058df83 230 MatrixXf Hpart = Hh*Hdq;
NaotoMorita 80:b241c058df83 231 MatrixXf H=MatrixXf::Zero(1,nState);
cocorlow 78:e36a7b844fb5 232 for(int j=0; j<3; j++){
cocorlow 78:e36a7b844fb5 233 H(0,j+6) = Hpart(0,j);
NaotoMorita 70:d12e46fdc2f0 234 }
NaotoMorita 70:d12e46fdc2f0 235
cocorlow 78:e36a7b844fb5 236 const float psi = std::atan2(qhat(1)*qhat(2) + qhat(0)*qhat(3), 0.5f - qhat(2)*qhat(2) - qhat(3)*qhat(3));
cocorlow 78:e36a7b844fb5 237 Matrix<float, 1, 1> z;
cocorlow 78:e36a7b844fb5 238 z << std::atan2(std::sin(a-psi), std::cos(a-psi));
cocorlow 78:e36a7b844fb5 239 MatrixXf K = (Phat*H.transpose())*(H*Phat*H.transpose()+R).inverse();
NaotoMorita 70:d12e46fdc2f0 240 errState = K * z;
cocorlow 78:e36a7b844fb5 241 Phat = (MatrixXf::Identity(nState, nState)-K*H)*Phat;
NaotoMorita 70:d12e46fdc2f0 242
NaotoMorita 70:d12e46fdc2f0 243 fuseErr2Nominal();
NaotoMorita 70:d12e46fdc2f0 244 }
NaotoMorita 80:b241c058df83 245
NaotoMorita 80:b241c058df83 246 void solaESKF::updateIMU(Vector3f acc,float heading, Matrix<float, 4, 4> R)
NaotoMorita 80:b241c058df83 247 {
NaotoMorita 80:b241c058df83 248
NaotoMorita 80:b241c058df83 249 Vector3f accm = acc - accBias;
NaotoMorita 80:b241c058df83 250 Matrix3f dcm;
NaotoMorita 80:b241c058df83 251 computeDcm(dcm, qhat);
NaotoMorita 80:b241c058df83 252 Matrix3f tdcm = dcm.transpose();
NaotoMorita 80:b241c058df83 253 Vector3f tdcm_g = tdcm*gravity;
NaotoMorita 80:b241c058df83 254 Matrix3f rotgrav = solaESKF::Matrixcross(tdcm_g);
NaotoMorita 80:b241c058df83 255
NaotoMorita 80:b241c058df83 256 MatrixXf H = MatrixXf::Zero(4,nState);
NaotoMorita 80:b241c058df83 257 for (int i = 0; i < 3; i++){
NaotoMorita 80:b241c058df83 258 for (int j = 0; j < 3; j++){
NaotoMorita 80:b241c058df83 259 H(i,j+6) = rotgrav(i,j);
NaotoMorita 80:b241c058df83 260 H(i,j+15) = tdcm(i,j);
NaotoMorita 80:b241c058df83 261 }
NaotoMorita 80:b241c058df83 262 }
NaotoMorita 80:b241c058df83 263
NaotoMorita 80:b241c058df83 264 H(0,9) = -1.0f;
NaotoMorita 80:b241c058df83 265 H(1,10) = -1.0f;
NaotoMorita 80:b241c058df83 266 H(2,11) = -1.0f;
NaotoMorita 80:b241c058df83 267
NaotoMorita 80:b241c058df83 268 float q0 = qhat(0);
NaotoMorita 80:b241c058df83 269 float q1 = qhat(1);
NaotoMorita 80:b241c058df83 270 float q2 = qhat(2);
NaotoMorita 80:b241c058df83 271 float q3 = qhat(3);
NaotoMorita 80:b241c058df83 272
NaotoMorita 80:b241c058df83 273 bool canUseA = false;
NaotoMorita 80:b241c058df83 274 const float SA0 = 2.0f*q3;
NaotoMorita 80:b241c058df83 275 const float SA1 = 2.0f*q2;
NaotoMorita 80:b241c058df83 276 const float SA2 = SA0*q0 + SA1*q1;
NaotoMorita 80:b241c058df83 277 const float SA3 = q0*q0 + q1*q1 - q2*q2 - q3*q3;
NaotoMorita 80:b241c058df83 278 float SA4, SA5_inv;
NaotoMorita 80:b241c058df83 279 if ((SA3*SA3) > 1e-6f) {
NaotoMorita 80:b241c058df83 280 SA4 = 1.0f/(SA3*SA3);
NaotoMorita 80:b241c058df83 281 SA5_inv = SA2*SA2*SA4 + 1.0f;
NaotoMorita 80:b241c058df83 282 canUseA = std::abs(SA5_inv) > 1e-6f;
NaotoMorita 80:b241c058df83 283 }
NaotoMorita 80:b241c058df83 284
NaotoMorita 80:b241c058df83 285 bool canUseB = false;
NaotoMorita 80:b241c058df83 286 const float SB0 = 2.0f*q0;
NaotoMorita 80:b241c058df83 287 const float SB1 = 2.0f*q1;
NaotoMorita 80:b241c058df83 288 const float SB2 = SB0*q3 + SB1*q2;
NaotoMorita 80:b241c058df83 289 const float SB4 = q0*q0 + q1*q1 - q2*q2 - q3*q3;
NaotoMorita 80:b241c058df83 290 float SB3, SB5_inv;
NaotoMorita 80:b241c058df83 291 if ((SB2*SB2) > 1e-6f) {
NaotoMorita 80:b241c058df83 292 SB3 = 1.0f/(SB2*SB2);
NaotoMorita 80:b241c058df83 293 SB5_inv = SB3*SB4*SB4 + 1;
NaotoMorita 80:b241c058df83 294 canUseB = std::abs(SB5_inv) > 1e-6f;
NaotoMorita 80:b241c058df83 295 }
NaotoMorita 80:b241c058df83 296
NaotoMorita 80:b241c058df83 297 MatrixXf Hh = MatrixXf::Zero(1,4);
NaotoMorita 80:b241c058df83 298
NaotoMorita 80:b241c058df83 299 if (canUseA && (!canUseB || std::abs(SA5_inv) >= std::abs(SB5_inv))) {
NaotoMorita 80:b241c058df83 300 const float SA5 = 1.0f/SA5_inv;
NaotoMorita 80:b241c058df83 301 const float SA6 = 1.0f/SA3;
NaotoMorita 80:b241c058df83 302 const float SA7 = SA2*SA4;
NaotoMorita 80:b241c058df83 303 const float SA8 = 2.0f*SA7;
NaotoMorita 80:b241c058df83 304 const float SA9 = 2.0f*SA6;
NaotoMorita 80:b241c058df83 305
NaotoMorita 80:b241c058df83 306 Hh(0,0) = SA5*(SA0*SA6 - SA8*q0);
NaotoMorita 80:b241c058df83 307 Hh(0,1) = SA5*(SA1*SA6 - SA8*q1);
NaotoMorita 80:b241c058df83 308 Hh(0,2) = SA5*(SA1*SA7 + SA9*q1);
NaotoMorita 80:b241c058df83 309 Hh(0,3) = SA5*(SA0*SA7 + SA9*q0);
NaotoMorita 80:b241c058df83 310 } else if (canUseB && (!canUseA || std::abs(SB5_inv) > std::abs(SA5_inv))) {
NaotoMorita 80:b241c058df83 311 const float SB5 = 1.0f/SB5_inv;
NaotoMorita 80:b241c058df83 312 const float SB6 = 1.0f/SB2;
NaotoMorita 80:b241c058df83 313 const float SB7 = SB3*SB4;
NaotoMorita 80:b241c058df83 314 const float SB8 = 2.0f*SB7;
NaotoMorita 80:b241c058df83 315 const float SB9 = 2.0f*SB6;
NaotoMorita 80:b241c058df83 316
NaotoMorita 80:b241c058df83 317 Hh(0,0) = -SB5*(SB0*SB6 - SB8*q3);
NaotoMorita 80:b241c058df83 318 Hh(0,1) = -SB5*(SB1*SB6 - SB8*q2);
NaotoMorita 80:b241c058df83 319 Hh(0,2) = -SB5*(-SB1*SB7 - SB9*q2);
NaotoMorita 80:b241c058df83 320 Hh(0,3) = -SB5*(-SB0*SB7 - SB9*q3);
NaotoMorita 80:b241c058df83 321 } else {
NaotoMorita 80:b241c058df83 322 return;
NaotoMorita 80:b241c058df83 323 }
NaotoMorita 80:b241c058df83 324
NaotoMorita 80:b241c058df83 325 MatrixXf Hdq = MatrixXf::Zero(4,3);
NaotoMorita 80:b241c058df83 326 Hdq << -0.5f*q1, -0.5f*q2, -0.5f*q3,
NaotoMorita 80:b241c058df83 327 0.5f*q0, -0.5f*q3, 0.5f*q2,
NaotoMorita 80:b241c058df83 328 0.5f*q3, 0.5f*q0, -0.5f*q1,
NaotoMorita 80:b241c058df83 329 -0.5f*q2, 0.5f*q1, 0.5f*q0;
NaotoMorita 80:b241c058df83 330
NaotoMorita 80:b241c058df83 331 MatrixXf Hpart = Hh*Hdq;
NaotoMorita 80:b241c058df83 332 for(int j=0; j<3; j++){
NaotoMorita 80:b241c058df83 333 H(3,j+6) = Hpart(0,j);
NaotoMorita 80:b241c058df83 334 }
NaotoMorita 80:b241c058df83 335
NaotoMorita 80:b241c058df83 336 const float psi = std::atan2(qhat(1)*qhat(2) + qhat(0)*qhat(3), 0.5f - qhat(2)*qhat(2) - qhat(3)*qhat(3));
NaotoMorita 80:b241c058df83 337 Vector3f zacc = -accm-tdcm*gravity;
NaotoMorita 80:b241c058df83 338 VectorXf z = VectorXf::Zero(4);
NaotoMorita 80:b241c058df83 339 z <<zacc(0),zacc(1),zacc(2),std::atan2(std::sin(heading-psi), std::cos(heading-psi));
NaotoMorita 80:b241c058df83 340 MatrixXf K = (Phat*H.transpose())*(H*Phat*H.transpose()+R).inverse();
NaotoMorita 80:b241c058df83 341 errState = K * z;
NaotoMorita 80:b241c058df83 342 Phat = (MatrixXf::Identity(nState, nState)-K*H)*Phat;
NaotoMorita 80:b241c058df83 343
NaotoMorita 80:b241c058df83 344 fuseErr2Nominal();
NaotoMorita 80:b241c058df83 345 }
cocorlow 78:e36a7b844fb5 346 void solaESKF::updateGPSPosition(Vector3f posgps,float palt,Matrix3f R)
NaotoMorita 65:c25d7810de44 347 {
NaotoMorita 80:b241c058df83 348 MatrixXf H = MatrixXf::Zero(3,nState);
cocorlow 78:e36a7b844fb5 349 H(0,0) = 1.0f;
NaotoMorita 74:f5fe7fecbd3c 350 H(1,1) = 1.0f;
NaotoMorita 74:f5fe7fecbd3c 351 H(2,2) = 1.0f;
NaotoMorita 65:c25d7810de44 352
NaotoMorita 74:f5fe7fecbd3c 353 //Matrix A = H*Phat*MatrixMath::Transpose(H)+R;
NaotoMorita 74:f5fe7fecbd3c 354 //Matrix K = (Phat*MatrixMath::Transpose(H))*(MatrixMath::Inv(MatrixMath::Transpose(A)*A+1000.0f*MatrixMath::Eye(3))*MatrixMath::Transpose(A));
cocorlow 78:e36a7b844fb5 355 MatrixXf K = (Phat*H.transpose())*(H*Phat*H.transpose()+R).inverse();
cocorlow 78:e36a7b844fb5 356 Vector3f z;
cocorlow 78:e36a7b844fb5 357 z << posgps(0)-pihat(0), posgps(1)-pihat(1), palt - pihat(2);
NaotoMorita 65:c25d7810de44 358 errState = K * z;
cocorlow 78:e36a7b844fb5 359 Phat = (MatrixXf::Identity(nState, nState)-K*H)*Phat;
NaotoMorita 74:f5fe7fecbd3c 360 //Phat = Phat-K*(H*Phat*MatrixMath::Transpose(H)+R)*MatrixMath::Transpose(K);
NaotoMorita 74:f5fe7fecbd3c 361 //Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*R*MatrixMath::Transpose(K);
NaotoMorita 65:c25d7810de44 362 fuseErr2Nominal();
NaotoMorita 65:c25d7810de44 363 }
cocorlow 78:e36a7b844fb5 364 void solaESKF::updateGPSVelocity(Vector3f velgps,Matrix3f R)
NaotoMorita 75:e2c825cdc511 365 {
NaotoMorita 80:b241c058df83 366 MatrixXf H = MatrixXf::Zero(3,nState);
cocorlow 78:e36a7b844fb5 367 H(0,3) = 1.0f;
NaotoMorita 75:e2c825cdc511 368 H(1,4) = 1.0f;
NaotoMorita 75:e2c825cdc511 369 H(2,5) = 1.0f;
NaotoMorita 75:e2c825cdc511 370
NaotoMorita 75:e2c825cdc511 371 //Matrix A = H*Phat*MatrixMath::Transpose(H)+R;
NaotoMorita 75:e2c825cdc511 372 //Matrix K = (Phat*MatrixMath::Transpose(H))*(MatrixMath::Inv(MatrixMath::Transpose(A)*A+1000.0f*MatrixMath::Eye(3))*MatrixMath::Transpose(A));
cocorlow 78:e36a7b844fb5 373 MatrixXf K = (Phat*H.transpose())*(H*Phat*H.transpose()+R).inverse();
cocorlow 78:e36a7b844fb5 374 Vector3f z;
cocorlow 78:e36a7b844fb5 375 z << velgps(0)-vihat(0), velgps(1)-vihat(1), velgps(2)-vihat(2);
NaotoMorita 75:e2c825cdc511 376 errState = K * z;
cocorlow 78:e36a7b844fb5 377 Phat = (MatrixXf::Identity(nState, nState)-K*H)*Phat;
NaotoMorita 75:e2c825cdc511 378 //Phat = Phat-K*(H*Phat*MatrixMath::Transpose(H)+R)*MatrixMath::Transpose(K);
NaotoMorita 75:e2c825cdc511 379 //Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*R*MatrixMath::Transpose(K);
NaotoMorita 75:e2c825cdc511 380 fuseErr2Nominal();
NaotoMorita 75:e2c825cdc511 381 }
NaotoMorita 65:c25d7810de44 382
NaotoMorita 80:b241c058df83 383 void solaESKF::updateWhole(Vector3f posgps, float palt, Vector3f velgps,Vector3f acc,float heading, MatrixXf R)
NaotoMorita 80:b241c058df83 384 {
NaotoMorita 80:b241c058df83 385 MatrixXf H = MatrixXf::Zero(9,nState);
NaotoMorita 80:b241c058df83 386 H(0,0) = 1.0f;
NaotoMorita 80:b241c058df83 387 H(1,1) = 1.0f;
NaotoMorita 80:b241c058df83 388 H(2,2) = 1.0f;
NaotoMorita 80:b241c058df83 389 H(3,3) = 1.0f;
NaotoMorita 80:b241c058df83 390 H(4,4) = 1.0f;
NaotoMorita 80:b241c058df83 391
NaotoMorita 80:b241c058df83 392 Vector3f accm = acc - accBias;
NaotoMorita 80:b241c058df83 393 Matrix3f dcm;
NaotoMorita 80:b241c058df83 394 computeDcm(dcm, qhat);
NaotoMorita 80:b241c058df83 395 Matrix3f tdcm = dcm.transpose();
NaotoMorita 80:b241c058df83 396 Vector3f tdcm_g = tdcm*gravity;
NaotoMorita 80:b241c058df83 397 Matrix3f rotgrav = solaESKF::Matrixcross(tdcm_g);
NaotoMorita 80:b241c058df83 398
NaotoMorita 80:b241c058df83 399 for (int i = 0; i < 3; i++){
NaotoMorita 80:b241c058df83 400 for (int j = 0; j < 3; j++){
NaotoMorita 80:b241c058df83 401 H(i+5,j+6) = rotgrav(i,j);
NaotoMorita 80:b241c058df83 402 H(i+5,j+15) = tdcm(i,j);
NaotoMorita 80:b241c058df83 403 }
NaotoMorita 80:b241c058df83 404 }
NaotoMorita 80:b241c058df83 405
NaotoMorita 80:b241c058df83 406 H(5,9) = -1.0f;
NaotoMorita 80:b241c058df83 407 H(6,10) = -1.0f;
NaotoMorita 80:b241c058df83 408 H(7,11) = -1.0f;
NaotoMorita 80:b241c058df83 409
NaotoMorita 80:b241c058df83 410 float q0 = qhat(0);
NaotoMorita 80:b241c058df83 411 float q1 = qhat(1);
NaotoMorita 80:b241c058df83 412 float q2 = qhat(2);
NaotoMorita 80:b241c058df83 413 float q3 = qhat(3);
NaotoMorita 80:b241c058df83 414
NaotoMorita 80:b241c058df83 415 bool canUseA = false;
NaotoMorita 80:b241c058df83 416 const float SA0 = 2.0f*q3;
NaotoMorita 80:b241c058df83 417 const float SA1 = 2.0f*q2;
NaotoMorita 80:b241c058df83 418 const float SA2 = SA0*q0 + SA1*q1;
NaotoMorita 80:b241c058df83 419 const float SA3 = q0*q0 + q1*q1 - q2*q2 - q3*q3;
NaotoMorita 80:b241c058df83 420 float SA4, SA5_inv;
NaotoMorita 80:b241c058df83 421 if ((SA3*SA3) > 1e-6f) {
NaotoMorita 80:b241c058df83 422 SA4 = 1.0f/(SA3*SA3);
NaotoMorita 80:b241c058df83 423 SA5_inv = SA2*SA2*SA4 + 1.0f;
NaotoMorita 80:b241c058df83 424 canUseA = std::abs(SA5_inv) > 1e-6f;
NaotoMorita 80:b241c058df83 425 }
NaotoMorita 80:b241c058df83 426
NaotoMorita 80:b241c058df83 427 bool canUseB = false;
NaotoMorita 80:b241c058df83 428 const float SB0 = 2.0f*q0;
NaotoMorita 80:b241c058df83 429 const float SB1 = 2.0f*q1;
NaotoMorita 80:b241c058df83 430 const float SB2 = SB0*q3 + SB1*q2;
NaotoMorita 80:b241c058df83 431 const float SB4 = q0*q0 + q1*q1 - q2*q2 - q3*q3;
NaotoMorita 80:b241c058df83 432 float SB3, SB5_inv;
NaotoMorita 80:b241c058df83 433 if ((SB2*SB2) > 1e-6f) {
NaotoMorita 80:b241c058df83 434 SB3 = 1.0f/(SB2*SB2);
NaotoMorita 80:b241c058df83 435 SB5_inv = SB3*SB4*SB4 + 1;
NaotoMorita 80:b241c058df83 436 canUseB = std::abs(SB5_inv) > 1e-6f;
NaotoMorita 80:b241c058df83 437 }
NaotoMorita 80:b241c058df83 438
NaotoMorita 80:b241c058df83 439 MatrixXf Hh = MatrixXf::Zero(1,4);
NaotoMorita 80:b241c058df83 440
NaotoMorita 80:b241c058df83 441 if (canUseA && (!canUseB || std::abs(SA5_inv) >= std::abs(SB5_inv))) {
NaotoMorita 80:b241c058df83 442 const float SA5 = 1.0f/SA5_inv;
NaotoMorita 80:b241c058df83 443 const float SA6 = 1.0f/SA3;
NaotoMorita 80:b241c058df83 444 const float SA7 = SA2*SA4;
NaotoMorita 80:b241c058df83 445 const float SA8 = 2.0f*SA7;
NaotoMorita 80:b241c058df83 446 const float SA9 = 2.0f*SA6;
NaotoMorita 80:b241c058df83 447
NaotoMorita 80:b241c058df83 448 Hh(0,0) = SA5*(SA0*SA6 - SA8*q0);
NaotoMorita 80:b241c058df83 449 Hh(0,1) = SA5*(SA1*SA6 - SA8*q1);
NaotoMorita 80:b241c058df83 450 Hh(0,2) = SA5*(SA1*SA7 + SA9*q1);
NaotoMorita 80:b241c058df83 451 Hh(0,3) = SA5*(SA0*SA7 + SA9*q0);
NaotoMorita 80:b241c058df83 452 } else if (canUseB && (!canUseA || std::abs(SB5_inv) > std::abs(SA5_inv))) {
NaotoMorita 80:b241c058df83 453 const float SB5 = 1.0f/SB5_inv;
NaotoMorita 80:b241c058df83 454 const float SB6 = 1.0f/SB2;
NaotoMorita 80:b241c058df83 455 const float SB7 = SB3*SB4;
NaotoMorita 80:b241c058df83 456 const float SB8 = 2.0f*SB7;
NaotoMorita 80:b241c058df83 457 const float SB9 = 2.0f*SB6;
NaotoMorita 80:b241c058df83 458
NaotoMorita 80:b241c058df83 459 Hh(0,0) = -SB5*(SB0*SB6 - SB8*q3);
NaotoMorita 80:b241c058df83 460 Hh(0,1) = -SB5*(SB1*SB6 - SB8*q2);
NaotoMorita 80:b241c058df83 461 Hh(0,2) = -SB5*(-SB1*SB7 - SB9*q2);
NaotoMorita 80:b241c058df83 462 Hh(0,3) = -SB5*(-SB0*SB7 - SB9*q3);
NaotoMorita 80:b241c058df83 463 } else {
NaotoMorita 80:b241c058df83 464 return;
NaotoMorita 80:b241c058df83 465 }
NaotoMorita 80:b241c058df83 466
NaotoMorita 80:b241c058df83 467 MatrixXf Hdq = MatrixXf::Zero(4,3);
NaotoMorita 80:b241c058df83 468 Hdq << -0.5f*q1, -0.5f*q2, -0.5f*q3,
NaotoMorita 80:b241c058df83 469 0.5f*q0, -0.5f*q3, 0.5f*q2,
NaotoMorita 80:b241c058df83 470 0.5f*q3, 0.5f*q0, -0.5f*q1,
NaotoMorita 80:b241c058df83 471 -0.5f*q2, 0.5f*q1, 0.5f*q0;
NaotoMorita 80:b241c058df83 472
NaotoMorita 80:b241c058df83 473 MatrixXf Hpart = Hh*Hdq;
NaotoMorita 80:b241c058df83 474 for(int j=0; j<3; j++){
NaotoMorita 80:b241c058df83 475 H(8,j+6) = Hpart(0,j);
NaotoMorita 80:b241c058df83 476 }
NaotoMorita 80:b241c058df83 477
NaotoMorita 80:b241c058df83 478 const float psi = std::atan2(qhat(1)*qhat(2) + qhat(0)*qhat(3), 0.5f - qhat(2)*qhat(2) - qhat(3)*qhat(3));
NaotoMorita 80:b241c058df83 479 Vector3f zacc = -accm-tdcm*gravity;
NaotoMorita 80:b241c058df83 480 VectorXf z = VectorXf::Zero(9);
NaotoMorita 80:b241c058df83 481 z << posgps(0)-pihat(0), posgps(1)-pihat(1), palt-pihat(2), velgps(0)-vihat(0), velgps(1)-vihat(1), zacc(0),zacc(1),zacc(2),std::atan2(std::sin(heading-psi), std::cos(heading-psi));
NaotoMorita 80:b241c058df83 482 MatrixXf K = (Phat*H.transpose())*(H*Phat*H.transpose()+R).inverse();
NaotoMorita 80:b241c058df83 483 errState = K * z;
NaotoMorita 80:b241c058df83 484 Phat = (MatrixXf::Identity(nState, nState)-K*H)*Phat;
NaotoMorita 80:b241c058df83 485
NaotoMorita 80:b241c058df83 486 fuseErr2Nominal();
NaotoMorita 80:b241c058df83 487
NaotoMorita 80:b241c058df83 488 }
NaotoMorita 80:b241c058df83 489
cocorlow 78:e36a7b844fb5 490 void solaESKF::updateGPS(Vector3f posgps, float palt, Vector3f velgps, MatrixXf R)
NaotoMorita 58:93ba28cf5cb3 491 {
NaotoMorita 80:b241c058df83 492 MatrixXf H = MatrixXf::Zero(5,nState);
cocorlow 78:e36a7b844fb5 493 H(0,0) = 1.0f;
NaotoMorita 46:15988dc41923 494 H(1,1) = 1.0f;
NaotoMorita 46:15988dc41923 495 H(2,2) = 1.0f;
NaotoMorita 56:c10f1168bd4a 496 H(3,3) = 1.0f;
NaotoMorita 56:c10f1168bd4a 497 H(4,4) = 1.0f;
cocorlow 78:e36a7b844fb5 498 MatrixXf K = (Phat*H.transpose())*(H*Phat*H.transpose()+R).inverse();
cocorlow 78:e36a7b844fb5 499 Matrix<float, 5, 1> z;
cocorlow 78:e36a7b844fb5 500 z << posgps(0)-pihat(0), posgps(1)-pihat(1), palt-pihat(2), velgps(0)-vihat(0), velgps(1)-vihat(1);
NaotoMorita 58:93ba28cf5cb3 501 errState = K * z;
cocorlow 78:e36a7b844fb5 502 Phat = (MatrixXf::Identity(nState, nState)-K*H)*Phat;
NaotoMorita 58:93ba28cf5cb3 503 fuseErr2Nominal();
NaotoMorita 58:93ba28cf5cb3 504 }
NaotoMorita 58:93ba28cf5cb3 505
NaotoMorita 46:15988dc41923 506
cocorlow 78:e36a7b844fb5 507 Vector3f solaESKF::computeAngles()
NaotoMorita 25:07ac5c6cd61c 508 {
NaotoMorita 25:07ac5c6cd61c 509
cocorlow 78:e36a7b844fb5 510 Vector3f euler;
cocorlow 78:e36a7b844fb5 511 euler(0) = std::atan2(qhat(0)*qhat(1) + qhat(2)*qhat(3), 0.5f - qhat(1)*qhat(1) - qhat(2)*qhat(2));
cocorlow 78:e36a7b844fb5 512 euler(1) = std::asin(-2.0f * (qhat(1)*qhat(3) - qhat(0)*qhat(2)));
cocorlow 78:e36a7b844fb5 513 euler(2) = std::atan2(qhat(1)*qhat(2) + qhat(0)*qhat(3), 0.5f - qhat(2)*qhat(2) - qhat(3)*qhat(3));
NaotoMorita 44:7d82e63b6a86 514 return euler;
NaotoMorita 19:3fae66745363 515 }
NaotoMorita 19:3fae66745363 516
NaotoMorita 21:d6079def0473 517
NaotoMorita 44:7d82e63b6a86 518 void solaESKF::fuseErr2Nominal()
NaotoMorita 19:3fae66745363 519 {
NaotoMorita 44:7d82e63b6a86 520 //position
cocorlow 78:e36a7b844fb5 521 pihat(0) += errState(0);
cocorlow 78:e36a7b844fb5 522 pihat(1) += errState(1);
cocorlow 78:e36a7b844fb5 523 pihat(2) += errState(2);
NaotoMorita 44:7d82e63b6a86 524
NaotoMorita 44:7d82e63b6a86 525 //velocity
cocorlow 78:e36a7b844fb5 526 vihat(0) += errState(3);
cocorlow 78:e36a7b844fb5 527 vihat(1) += errState(4);
cocorlow 78:e36a7b844fb5 528 vihat(2) += errState(5);
NaotoMorita 44:7d82e63b6a86 529
NaotoMorita 44:7d82e63b6a86 530 //angle error
cocorlow 78:e36a7b844fb5 531 Vector4f qerr;
cocorlow 78:e36a7b844fb5 532 qerr << 1.0f, 0.5f*errState(6), 0.5f*errState(7), 0.5f*errState(8);
NaotoMorita 19:3fae66745363 533 qhat = quatmultiply(qhat, qerr);
cocorlow 78:e36a7b844fb5 534 qhat.normalize();
NaotoMorita 23:1509648c2318 535
NaotoMorita 44:7d82e63b6a86 536 //acc bias
cocorlow 78:e36a7b844fb5 537 accBias(0) += errState(9);
cocorlow 78:e36a7b844fb5 538 accBias(1) += errState(10);
cocorlow 78:e36a7b844fb5 539 accBias(2) += errState(11);
NaotoMorita 44:7d82e63b6a86 540
NaotoMorita 44:7d82e63b6a86 541 //gyro bias
cocorlow 78:e36a7b844fb5 542 gyroBias(0) += errState(12);
cocorlow 78:e36a7b844fb5 543 gyroBias(1) += errState(13);
cocorlow 78:e36a7b844fb5 544 gyroBias(2) += errState(14);
NaotoMorita 44:7d82e63b6a86 545
NaotoMorita 44:7d82e63b6a86 546 //gravity bias
cocorlow 78:e36a7b844fb5 547 gravity(0) += errState(15);
cocorlow 78:e36a7b844fb5 548 gravity(1) += errState(16);
cocorlow 78:e36a7b844fb5 549 gravity(2) += errState(17);
NaotoMorita 62:5519d34eb6e8 550
cocorlow 78:e36a7b844fb5 551 errState = VectorXf::Zero(nState);
NaotoMorita 19:3fae66745363 552 }
NaotoMorita 19:3fae66745363 553
cocorlow 78:e36a7b844fb5 554 Vector4f solaESKF::quatmultiply(Vector4f p, Vector4f q)
NaotoMorita 19:3fae66745363 555 {
cocorlow 78:e36a7b844fb5 556 Vector4f qout;
cocorlow 78:e36a7b844fb5 557 qout(0) = p(0)*q(0) - p(1)*q(1) - p(2)*q(2) - p(3)*q(3);
cocorlow 78:e36a7b844fb5 558 qout(1) = p(0)*q(1) + p(1)*q(0) + p(2)*q(3) - p(3)*q(2);
cocorlow 78:e36a7b844fb5 559 qout(2) = p(0)*q(2) - p(1)*q(3) + p(2)*q(0) + p(3)*q(1);
cocorlow 78:e36a7b844fb5 560 qout(3) = p(0)*q(3) + p(1)*q(2) - p(2)*q(1) + p(3)*q(0);
cocorlow 78:e36a7b844fb5 561 // qout.normalize();
NaotoMorita 19:3fae66745363 562 return qout;
NaotoMorita 19:3fae66745363 563 }
NaotoMorita 19:3fae66745363 564
cocorlow 78:e36a7b844fb5 565 void solaESKF::computeDcm(Matrix3f& dcm, Vector4f quat)
NaotoMorita 19:3fae66745363 566 {
cocorlow 78:e36a7b844fb5 567 // 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);
cocorlow 78:e36a7b844fb5 568 // dcm(1,2) = 2.0f*(quat(2,1)*quat(3,1) - quat(1,1)*quat(4,1));
cocorlow 78:e36a7b844fb5 569 // dcm(1,3) = 2.0f*(quat(2,1)*quat(4,1) + quat(1,1)*quat(3,1));
cocorlow 78:e36a7b844fb5 570 // dcm(2,1) = 2.0f*(quat(2,1)*quat(3,1) + quat(1,1)*quat(4,1));
cocorlow 78:e36a7b844fb5 571 // 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);
cocorlow 78:e36a7b844fb5 572 // dcm(2,3) = 2.0f*(quat(3,1)*quat(4,1) - quat(1,1)*quat(2,1));
cocorlow 78:e36a7b844fb5 573 // dcm(3,1) = 2.0f*(quat(2,1)*quat(4,1) - quat(1,1)*quat(3,1));
cocorlow 78:e36a7b844fb5 574 // dcm(3,2) = 2.0f*(quat(3,1)*quat(4,1) + quat(1,1)*quat(2,1));
cocorlow 78:e36a7b844fb5 575 // 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);
cocorlow 78:e36a7b844fb5 576
cocorlow 78:e36a7b844fb5 577 dcm(0,0) = quat(0)*quat(0) + quat(1)*quat(1) - quat(2)*quat(2) - quat(3)*quat(3);
cocorlow 78:e36a7b844fb5 578 dcm(0,1) = 2.0f*(quat(1)*quat(2) - quat(0)*quat(3));
cocorlow 78:e36a7b844fb5 579 dcm(0,2) = 2.0f*(quat(1)*quat(3) + quat(0)*quat(2));
cocorlow 78:e36a7b844fb5 580 dcm(1,0) = 2.0f*(quat(1)*quat(2) + quat(0)*quat(3));
cocorlow 78:e36a7b844fb5 581 dcm(1,1) = quat(0)*quat(0) - quat(1)*quat(1) + quat(2)*quat(2) - quat(3)*quat(3);
cocorlow 78:e36a7b844fb5 582 dcm(1,2) = 2.0f*(quat(2)*quat(3) - quat(0)*quat(1));
cocorlow 78:e36a7b844fb5 583 dcm(2,0) = 2.0f*(quat(1)*quat(3) - quat(0)*quat(2));
cocorlow 78:e36a7b844fb5 584 dcm(2,1) = 2.0f*(quat(2)*quat(3) + quat(0)*quat(1));
cocorlow 78:e36a7b844fb5 585 dcm(2,2) = quat(0)*quat(0) - quat(1)*quat(1) - quat(2)*quat(2) + quat(3)*quat(3);
NaotoMorita 19:3fae66745363 586 }
NaotoMorita 19:3fae66745363 587
NaotoMorita 44:7d82e63b6a86 588 void solaESKF::setQhat(float ex,float ey,float ez)
NaotoMorita 44:7d82e63b6a86 589 {
cocorlow 78:e36a7b844fb5 590 float cos_z_2 = std::cos(0.5f*ez);
cocorlow 78:e36a7b844fb5 591 float cos_y_2 = std::cos(0.5f*ey);
cocorlow 78:e36a7b844fb5 592 float cos_x_2 = std::cos(0.5f*ex);
NaotoMorita 19:3fae66745363 593
cocorlow 78:e36a7b844fb5 594 float sin_z_2 = std::sin(0.5f*ez);
cocorlow 78:e36a7b844fb5 595 float sin_y_2 = std::sin(0.5f*ey);
cocorlow 78:e36a7b844fb5 596 float sin_x_2 = std::sin(0.5f*ex);
NaotoMorita 19:3fae66745363 597
NaotoMorita 19:3fae66745363 598 // and now compute quaternion
cocorlow 78:e36a7b844fb5 599 qhat(0) = cos_z_2*cos_y_2*cos_x_2 + sin_z_2*sin_y_2*sin_x_2;
cocorlow 78:e36a7b844fb5 600 qhat(1) = cos_z_2*cos_y_2*sin_x_2 - sin_z_2*sin_y_2*cos_x_2;
cocorlow 78:e36a7b844fb5 601 qhat(2) = cos_z_2*sin_y_2*cos_x_2 + sin_z_2*cos_y_2*sin_x_2;
cocorlow 78:e36a7b844fb5 602 qhat(3) = sin_z_2*cos_y_2*cos_x_2 - cos_z_2*sin_y_2*sin_x_2;
NaotoMorita 19:3fae66745363 603 }
NaotoMorita 19:3fae66745363 604
cocorlow 78:e36a7b844fb5 605 Vector3f solaESKF::calcDynAcc(Vector3f acc)
NaotoMorita 75:e2c825cdc511 606 {
cocorlow 78:e36a7b844fb5 607 Vector3f accm = acc - accBias;
NaotoMorita 80:b241c058df83 608 Matrix3f dcm;
NaotoMorita 80:b241c058df83 609 computeDcm(dcm, qhat);
NaotoMorita 80:b241c058df83 610 Matrix3f tdcm = dcm.transpose();
NaotoMorita 75:e2c825cdc511 611
cocorlow 78:e36a7b844fb5 612 Vector3f dynAcc = accm+tdcm*gravity;
NaotoMorita 75:e2c825cdc511 613 return dynAcc;
NaotoMorita 75:e2c825cdc511 614 }
NaotoMorita 47:2467de40951f 615
NaotoMorita 47:2467de40951f 616
NaotoMorita 44:7d82e63b6a86 617 void solaESKF::setGravity(float gx,float gy,float gz)
NaotoMorita 23:1509648c2318 618 {
cocorlow 78:e36a7b844fb5 619 gravity(0) = gx;
cocorlow 78:e36a7b844fb5 620 gravity(1) = gy;
cocorlow 78:e36a7b844fb5 621 gravity(2) = gz;
NaotoMorita 23:1509648c2318 622 }
NaotoMorita 62:5519d34eb6e8 623
cocorlow 78:e36a7b844fb5 624 Vector3f solaESKF::getPihat()
NaotoMorita 47:2467de40951f 625 {
NaotoMorita 47:2467de40951f 626 return pihat;
NaotoMorita 47:2467de40951f 627 }
cocorlow 78:e36a7b844fb5 628 Vector3f solaESKF::getVihat()
NaotoMorita 47:2467de40951f 629 {
NaotoMorita 47:2467de40951f 630 return vihat;
NaotoMorita 47:2467de40951f 631 }
cocorlow 78:e36a7b844fb5 632 Vector4f solaESKF::getQhat()
NaotoMorita 47:2467de40951f 633 {
NaotoMorita 47:2467de40951f 634 return qhat;
NaotoMorita 47:2467de40951f 635 }
cocorlow 78:e36a7b844fb5 636 Vector3f solaESKF::getAccBias()
NaotoMorita 47:2467de40951f 637 {
NaotoMorita 47:2467de40951f 638 return accBias;
NaotoMorita 47:2467de40951f 639 }
cocorlow 78:e36a7b844fb5 640 Vector3f solaESKF::getGyroBias()
NaotoMorita 47:2467de40951f 641 {
NaotoMorita 47:2467de40951f 642 return gyroBias;
NaotoMorita 47:2467de40951f 643 }
cocorlow 78:e36a7b844fb5 644 Vector3f solaESKF::getGravity()
NaotoMorita 47:2467de40951f 645 {
NaotoMorita 47:2467de40951f 646 return gravity;
NaotoMorita 47:2467de40951f 647 }
NaotoMorita 74:f5fe7fecbd3c 648
cocorlow 78:e36a7b844fb5 649 VectorXf solaESKF::getErrState()
NaotoMorita 47:2467de40951f 650 {
NaotoMorita 47:2467de40951f 651 return errState;
NaotoMorita 47:2467de40951f 652 }
NaotoMorita 23:1509648c2318 653
NaotoMorita 80:b241c058df83 654 VectorXf solaESKF::getState()
NaotoMorita 47:2467de40951f 655 {
NaotoMorita 80:b241c058df83 656 VectorXf state = VectorXf::Zero(nState+1);
NaotoMorita 80:b241c058df83 657 for (int i = 0; i < 3; i++){
NaotoMorita 80:b241c058df83 658 state(i) = pihat(i);
NaotoMorita 80:b241c058df83 659 state(i+3) = vihat(i);
NaotoMorita 80:b241c058df83 660 }
NaotoMorita 80:b241c058df83 661 for (int i = 0; i < 4; i++){
NaotoMorita 80:b241c058df83 662 state(i+6) = qhat(i);
NaotoMorita 80:b241c058df83 663 }
NaotoMorita 80:b241c058df83 664 for (int i = 0; i < 3; i++){
NaotoMorita 80:b241c058df83 665 state(i+10) = accBias(i);
NaotoMorita 80:b241c058df83 666 state(i+13) = gyroBias(i);
NaotoMorita 80:b241c058df83 667 state(i+16) = gravity(i);
NaotoMorita 80:b241c058df83 668 }
NaotoMorita 80:b241c058df83 669 return state;
NaotoMorita 47:2467de40951f 670 }
NaotoMorita 80:b241c058df83 671
NaotoMorita 80:b241c058df83 672 VectorXf solaESKF::getVariance()
NaotoMorita 47:2467de40951f 673 {
NaotoMorita 80:b241c058df83 674 VectorXf variance = VectorXf::Zero(nState);
NaotoMorita 80:b241c058df83 675 for (int i = 0; i < nState; i++){
NaotoMorita 80:b241c058df83 676 variance(i) = Phat(i,i);
NaotoMorita 80:b241c058df83 677 }
NaotoMorita 80:b241c058df83 678 return variance;
NaotoMorita 80:b241c058df83 679 }
NaotoMorita 80:b241c058df83 680
NaotoMorita 80:b241c058df83 681 void solaESKF::setPhatPosition(float valNE,float valD)
NaotoMorita 80:b241c058df83 682 {
NaotoMorita 80:b241c058df83 683 setBlockDiag(Phat, valNE, 0,2);
NaotoMorita 80:b241c058df83 684 Phat(2,2) = valD;
NaotoMorita 80:b241c058df83 685 }
NaotoMorita 80:b241c058df83 686 void solaESKF::setPhatVelocity(float valNE,float valD)
NaotoMorita 80:b241c058df83 687 {
NaotoMorita 80:b241c058df83 688 setBlockDiag(Phat, valNE, 3,5);
NaotoMorita 80:b241c058df83 689 Phat(5,5) = valD;
NaotoMorita 47:2467de40951f 690 }
NaotoMorita 47:2467de40951f 691 void solaESKF::setPhatAngleError(float val)
NaotoMorita 47:2467de40951f 692 {
cocorlow 78:e36a7b844fb5 693 setBlockDiag(Phat, val, 6,8);
NaotoMorita 47:2467de40951f 694 }
NaotoMorita 47:2467de40951f 695 void solaESKF::setPhatAccBias(float val)
NaotoMorita 47:2467de40951f 696 {
cocorlow 78:e36a7b844fb5 697 setBlockDiag(Phat, val, 9,11);
NaotoMorita 47:2467de40951f 698 }
NaotoMorita 47:2467de40951f 699 void solaESKF::setPhatGyroBias(float val)
NaotoMorita 47:2467de40951f 700 {
cocorlow 78:e36a7b844fb5 701 setBlockDiag(Phat, val, 12,14);
NaotoMorita 47:2467de40951f 702 }
NaotoMorita 47:2467de40951f 703 void solaESKF::setPhatGravity(float val)
NaotoMorita 47:2467de40951f 704 {
cocorlow 78:e36a7b844fb5 705 setBlockDiag(Phat, val, 15,17);
NaotoMorita 62:5519d34eb6e8 706 }
NaotoMorita 47:2467de40951f 707
NaotoMorita 47:2467de40951f 708
NaotoMorita 80:b241c058df83 709 void solaESKF::setQVelocity(float valNE,float valD)
NaotoMorita 47:2467de40951f 710 {
NaotoMorita 80:b241c058df83 711 setBlockDiag(Q, valNE, 3, 5);
NaotoMorita 80:b241c058df83 712 Q(5,5) = valD;
NaotoMorita 47:2467de40951f 713 }
NaotoMorita 47:2467de40951f 714 void solaESKF::setQAngleError(float val)
NaotoMorita 47:2467de40951f 715 {
cocorlow 78:e36a7b844fb5 716 setBlockDiag(Q, val, 6, 8);
NaotoMorita 47:2467de40951f 717 }
NaotoMorita 47:2467de40951f 718 void solaESKF::setQAccBias(float val)
NaotoMorita 47:2467de40951f 719 {
cocorlow 78:e36a7b844fb5 720 setBlockDiag(Q, val, 9, 11);
NaotoMorita 47:2467de40951f 721 }
NaotoMorita 47:2467de40951f 722 void solaESKF::setQGyroBias(float val)
NaotoMorita 47:2467de40951f 723 {
cocorlow 78:e36a7b844fb5 724 setBlockDiag(Q, val, 12, 14);
NaotoMorita 47:2467de40951f 725 }
NaotoMorita 19:3fae66745363 726
NaotoMorita 80:b241c058df83 727 void solaESKF::setDiag(Matrix3f& mat, float val){
NaotoMorita 80:b241c058df83 728 for (int i = 0; i < mat.cols(); i++){
NaotoMorita 80:b241c058df83 729 for (int j = 0; j < mat.rows(); j++){
NaotoMorita 80:b241c058df83 730 mat(i,j) = 0.0f;
NaotoMorita 80:b241c058df83 731 }
NaotoMorita 80:b241c058df83 732 }
cocorlow 78:e36a7b844fb5 733 for (int i = 0; i < mat.cols(); i++){
NaotoMorita 44:7d82e63b6a86 734 mat(i,i) = val;
NaotoMorita 22:7d84b8bc20b4 735 }
NaotoMorita 22:7d84b8bc20b4 736 }
NaotoMorita 22:7d84b8bc20b4 737
NaotoMorita 80:b241c058df83 738 void solaESKF::setDiag(MatrixXf& mat, float val){
NaotoMorita 80:b241c058df83 739 for (int i = 0; i < mat.cols(); i++){
NaotoMorita 80:b241c058df83 740 for (int j = 0; j < mat.rows(); j++){
NaotoMorita 80:b241c058df83 741 mat(i,j) = 0.0f;
NaotoMorita 80:b241c058df83 742 }
NaotoMorita 80:b241c058df83 743 }
NaotoMorita 80:b241c058df83 744 for (int i = 0; i < mat.cols(); i++)
NaotoMorita 80:b241c058df83 745 {
NaotoMorita 80:b241c058df83 746 mat(i, i) = val;
NaotoMorita 80:b241c058df83 747 }
NaotoMorita 80:b241c058df83 748 }
NaotoMorita 80:b241c058df83 749
cocorlow 78:e36a7b844fb5 750 void solaESKF::setBlockDiag(MatrixXf& mat, float val, int startIndex, int endIndex){
NaotoMorita 80:b241c058df83 751
NaotoMorita 44:7d82e63b6a86 752 for (int i = startIndex; i < endIndex+1; i++){
NaotoMorita 44:7d82e63b6a86 753 mat(i,i) = val;
NaotoMorita 22:7d84b8bc20b4 754 }
NaotoMorita 22:7d84b8bc20b4 755 }
osaka 51:a5af3b280d23 756
NaotoMorita 58:93ba28cf5cb3 757 void solaESKF::setPihat(float pi_x, float pi_y)
osaka 51:a5af3b280d23 758 {
cocorlow 78:e36a7b844fb5 759 pihat(0) = pi_x;
cocorlow 78:e36a7b844fb5 760 pihat(1) = pi_y;
NaotoMorita 58:93ba28cf5cb3 761 //pihat(3,1) = pi_z;
NaotoMorita 56:c10f1168bd4a 762 }
NaotoMorita 73:5770a0d470c0 763
cocorlow 78:e36a7b844fb5 764 Matrix3f solaESKF::Matrixcross(Vector3f v)
cocorlow 78:e36a7b844fb5 765 {
cocorlow 78:e36a7b844fb5 766 Matrix3f m;
cocorlow 78:e36a7b844fb5 767 m << 0.0f, -v(2), v(1),
cocorlow 78:e36a7b844fb5 768 v(2), 0.0f, -v(0),
cocorlow 78:e36a7b844fb5 769 -v(1), v(0), 0.0f;
cocorlow 78:e36a7b844fb5 770 return m;
cocorlow 78:e36a7b844fb5 771 }
NaotoMorita 56:c10f1168bd4a 772 /*
NaotoMorita 56:c10f1168bd4a 773 void solaESKF::updateAccConstraints(Matrix acc,float palt,Matrix R)
NaotoMorita 56:c10f1168bd4a 774 {
NaotoMorita 56:c10f1168bd4a 775 Matrix accm = acc - accBias;
NaotoMorita 56:c10f1168bd4a 776 Matrix tdcm(3,3);
NaotoMorita 56:c10f1168bd4a 777 computeDcm(tdcm, qhat);
NaotoMorita 56:c10f1168bd4a 778 tdcm = MatrixMath::Transpose(tdcm);
NaotoMorita 56:c10f1168bd4a 779 Matrix tdcm_g = tdcm*gravity;
NaotoMorita 56:c10f1168bd4a 780 Matrix a2v = MatrixMath::Matrixcross(tdcm_g(1,1),tdcm_g(2,1),tdcm_g(3,1));
NaotoMorita 56:c10f1168bd4a 781
NaotoMorita 56:c10f1168bd4a 782 Matrix H(4,nState);
NaotoMorita 56:c10f1168bd4a 783 for (int i = 1; i < 4; i++){
NaotoMorita 56:c10f1168bd4a 784 for (int j = 1; j < 4; j++){
NaotoMorita 56:c10f1168bd4a 785 H(i,j+6) = a2v(i,j);
NaotoMorita 56:c10f1168bd4a 786 H(i,j+15) = tdcm(i,j);
NaotoMorita 56:c10f1168bd4a 787 }
NaotoMorita 56:c10f1168bd4a 788 }
NaotoMorita 56:c10f1168bd4a 789 H(1,10) = -1.0f;
NaotoMorita 56:c10f1168bd4a 790 H(2,11) = -1.0f;
NaotoMorita 56:c10f1168bd4a 791 H(3,12) = -1.0f;
NaotoMorita 56:c10f1168bd4a 792 H(4,3) = 1.0f;
NaotoMorita 56:c10f1168bd4a 793
NaotoMorita 56:c10f1168bd4a 794 //Matrix A = H*Phat*MatrixMath::Transpose(H)+R;
NaotoMorita 56:c10f1168bd4a 795 //Matrix K = (Phat*MatrixMath::Transpose(H))*(MatrixMath::Inv(MatrixMath::Transpose(A)*A+10.0f*MatrixMath::Eye(3))*MatrixMath::Transpose(A));
NaotoMorita 56:c10f1168bd4a 796 Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
NaotoMorita 56:c10f1168bd4a 797 Matrix zacc = -accm-tdcm*gravity;
NaotoMorita 56:c10f1168bd4a 798 Matrix z(4,1);
NaotoMorita 56:c10f1168bd4a 799 z << zacc(1,1) << zacc(2,1) << zacc(3,1) << palt - pihat(3,1);
NaotoMorita 56:c10f1168bd4a 800 errState = K * z;
NaotoMorita 56:c10f1168bd4a 801 Phat = (MatrixMath::Eye(nState)-K*H)*Phat;
NaotoMorita 56:c10f1168bd4a 802 //Phat = Phat-K*(H*Phat*MatrixMath::Transpose(H)+R)*MatrixMath::Transpose(K);
NaotoMorita 56:c10f1168bd4a 803 //Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*R*MatrixMath::Transpose(K);
NaotoMorita 56:c10f1168bd4a 804 fuseErr2Nominal();
NaotoMorita 56:c10f1168bd4a 805 }
NaotoMorita 56:c10f1168bd4a 806
NaotoMorita 56:c10f1168bd4a 807 void solaESKF::updateGyroConstraints(Matrix gyro,Matrix R)
NaotoMorita 56:c10f1168bd4a 808 {
NaotoMorita 56:c10f1168bd4a 809 Matrix gyrom = gyro - gyroBias;
NaotoMorita 56:c10f1168bd4a 810 Matrix dcm(3,3);
NaotoMorita 56:c10f1168bd4a 811 computeDcm(dcm, qhat);
NaotoMorita 56:c10f1168bd4a 812 Matrix a2v = dcm*MatrixMath::Matrixcross(gyrom(1,1),gyrom(2,1),gyrom(3,1));
NaotoMorita 56:c10f1168bd4a 813
NaotoMorita 56:c10f1168bd4a 814 Matrix H(2,nState);
NaotoMorita 56:c10f1168bd4a 815 for (int i = 1; i < 3; i++){
NaotoMorita 56:c10f1168bd4a 816 for (int j = 1; j < 4; j++){
NaotoMorita 56:c10f1168bd4a 817 H(i,j+6) = a2v(i,j);
NaotoMorita 56:c10f1168bd4a 818 H(i,j+12) = -dcm(i,j);
NaotoMorita 56:c10f1168bd4a 819 }
NaotoMorita 56:c10f1168bd4a 820 }
NaotoMorita 56:c10f1168bd4a 821 //Matrix A = H*Phat*MatrixMath::Transpose(H)+R;
NaotoMorita 56:c10f1168bd4a 822 //Matrix K = (Phat*MatrixMath::Transpose(H))*(MatrixMath::Inv(MatrixMath::Transpose(A)*A+10.0f*MatrixMath::Eye(2))*MatrixMath::Transpose(A));
NaotoMorita 56:c10f1168bd4a 823 Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
NaotoMorita 56:c10f1168bd4a 824
NaotoMorita 56:c10f1168bd4a 825 Matrix z3 = -dcm*gyrom;
NaotoMorita 56:c10f1168bd4a 826 Matrix z(2,1);
NaotoMorita 56:c10f1168bd4a 827 z << z3(1,1) << z3(2,1);
NaotoMorita 56:c10f1168bd4a 828 errState = K * z;
NaotoMorita 56:c10f1168bd4a 829 //Phat = (MatrixMath::Eye(nState)-K*H)*Phat;
NaotoMorita 56:c10f1168bd4a 830 //Phat = Phat-K*(H*Phat*MatrixMath::Transpose(H)+R)*MatrixMath::Transpose(K);
NaotoMorita 56:c10f1168bd4a 831 Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*R*MatrixMath::Transpose(K);
NaotoMorita 56:c10f1168bd4a 832 fuseErr2Nominal();
NaotoMorita 56:c10f1168bd4a 833 }
NaotoMorita 56:c10f1168bd4a 834 void solaESKF::updateMag(Matrix mag, Matrix R)
NaotoMorita 56:c10f1168bd4a 835 {
NaotoMorita 56:c10f1168bd4a 836 Matrix dcm(3,3);
NaotoMorita 56:c10f1168bd4a 837 computeDcm(dcm, qhat);
NaotoMorita 56:c10f1168bd4a 838 Matrix a2v = -dcm*MatrixMath::Matrixcross(mag(1,1),mag(2,1),mag(3,1));
NaotoMorita 56:c10f1168bd4a 839
NaotoMorita 56:c10f1168bd4a 840 Matrix H(2,nState);
NaotoMorita 56:c10f1168bd4a 841 for (int i = 1; i < 3; i++){
NaotoMorita 56:c10f1168bd4a 842 for (int j = 1; j < 4; j++){
NaotoMorita 56:c10f1168bd4a 843 H(i,j+6) = a2v(i,j);
NaotoMorita 56:c10f1168bd4a 844 }
NaotoMorita 56:c10f1168bd4a 845 }
NaotoMorita 56:c10f1168bd4a 846 H(1,19) = 1.0f;
NaotoMorita 56:c10f1168bd4a 847 //H(3,20) = 1.0f;
NaotoMorita 56:c10f1168bd4a 848
NaotoMorita 56:c10f1168bd4a 849 //Matrix A = H*Phat*MatrixMath::Transpose(H)+R;
NaotoMorita 56:c10f1168bd4a 850 //Matrix K = (Phat*MatrixMath::Transpose(H))*(MatrixMath::Inv(MatrixMath::Transpose(A)*A+10.0f*MatrixMath::Eye(3))*MatrixMath::Transpose(A));
NaotoMorita 56:c10f1168bd4a 851 Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
NaotoMorita 56:c10f1168bd4a 852 Matrix zmag = -dcm*mag-magField;
NaotoMorita 56:c10f1168bd4a 853 Matrix z(2,1);
NaotoMorita 56:c10f1168bd4a 854 z << zmag(1,1) << zmag(2,1);
NaotoMorita 56:c10f1168bd4a 855 errState = K * z;
NaotoMorita 56:c10f1168bd4a 856 Phat = (MatrixMath::Eye(nState)-K*H)*Phat;
NaotoMorita 56:c10f1168bd4a 857 //Phat = Phat-K*(H*Phat*MatrixMath::Transpose(H)+R)*MatrixMath::Transpose(K);
NaotoMorita 56:c10f1168bd4a 858 //Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*R*MatrixMath::Transpose(K);
NaotoMorita 56:c10f1168bd4a 859 fuseErr2Nominal();
NaotoMorita 56:c10f1168bd4a 860 }
NaotoMorita 59:03fe5e16a33c 861 void solaESKF::updateMag(Matrix mag,float palt, Matrix R)
NaotoMorita 59:03fe5e16a33c 862 {
NaotoMorita 59:03fe5e16a33c 863 Matrix dcm(3,3);
NaotoMorita 59:03fe5e16a33c 864 computeDcm(dcm, qhat);
NaotoMorita 59:03fe5e16a33c 865 Matrix a2v = -dcm*MatrixMath::Matrixcross(mag(1,1),mag(2,1),mag(3,1));
NaotoMorita 59:03fe5e16a33c 866
NaotoMorita 59:03fe5e16a33c 867 Matrix H(3,nState);
NaotoMorita 59:03fe5e16a33c 868 for (int i = 1; i < 3; i++){
NaotoMorita 59:03fe5e16a33c 869 for (int j = 1; j < 4; j++){
NaotoMorita 59:03fe5e16a33c 870 H(i,j+6) = a2v(i,j);
NaotoMorita 59:03fe5e16a33c 871 }
NaotoMorita 59:03fe5e16a33c 872 }
NaotoMorita 59:03fe5e16a33c 873 H(1,19) = 1.0f;
NaotoMorita 59:03fe5e16a33c 874 H(3,3) = 1.0f;
NaotoMorita 59:03fe5e16a33c 875 //Matrix A = H*Phat*MatrixMath::Transpose(H)+R;
NaotoMorita 59:03fe5e16a33c 876 //Matrix K = (Phat*MatrixMath::Transpose(H))*(MatrixMath::Inv(MatrixMath::Transpose(A)*A+10.0f*MatrixMath::Eye(3))*MatrixMath::Transpose(A));
NaotoMorita 59:03fe5e16a33c 877 Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
NaotoMorita 59:03fe5e16a33c 878 Matrix zmag = -dcm*mag-magField;
NaotoMorita 59:03fe5e16a33c 879 Matrix z(3,1);
NaotoMorita 59:03fe5e16a33c 880 z << zmag(1,1) << zmag(2,1) << palt - pihat(3,1);
NaotoMorita 59:03fe5e16a33c 881 errState = K * z;
NaotoMorita 59:03fe5e16a33c 882 Phat = (MatrixMath::Eye(nState)-K*H)*Phat;
NaotoMorita 59:03fe5e16a33c 883 //Phat = Phat-K*(H*Phat*MatrixMath::Transpose(H)+R)*MatrixMath::Transpose(K);
NaotoMorita 59:03fe5e16a33c 884 //Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*R*MatrixMath::Transpose(K);
NaotoMorita 59:03fe5e16a33c 885
NaotoMorita 59:03fe5e16a33c 886 fuseErr2Nominal();
NaotoMorita 59:03fe5e16a33c 887 }
NaotoMorita 59:03fe5e16a33c 888 void solaESKF::updateGPSVelocity(Matrix velgps,Matrix mag,Matrix R)
NaotoMorita 59:03fe5e16a33c 889 {
NaotoMorita 59:03fe5e16a33c 890
NaotoMorita 59:03fe5e16a33c 891 Matrix dcm(3,3);
NaotoMorita 59:03fe5e16a33c 892 computeDcm(dcm, qhat);
NaotoMorita 59:03fe5e16a33c 893 Matrix a2v = -dcm*MatrixMath::Matrixcross(mag(1,1),mag(2,1),mag(3,1));
NaotoMorita 59:03fe5e16a33c 894
NaotoMorita 59:03fe5e16a33c 895
NaotoMorita 59:03fe5e16a33c 896 Matrix H(3,nState);
NaotoMorita 59:03fe5e16a33c 897 H(1,4) = 1.0f;
NaotoMorita 59:03fe5e16a33c 898 H(2,5) = 1.0f;
NaotoMorita 59:03fe5e16a33c 899
NaotoMorita 59:03fe5e16a33c 900 for (int j = 1; j < 4; j++){
NaotoMorita 59:03fe5e16a33c 901 H(3,j+6) = a2v(2,j);
NaotoMorita 59:03fe5e16a33c 902 }
NaotoMorita 59:03fe5e16a33c 903 //H(3,19) = 1.0f;
NaotoMorita 59:03fe5e16a33c 904
NaotoMorita 59:03fe5e16a33c 905 Matrix zmag = -dcm*mag-magField;
NaotoMorita 59:03fe5e16a33c 906
NaotoMorita 59:03fe5e16a33c 907 //Matrix A = H*Phat*MatrixMath::Transpose(H)+R;
NaotoMorita 59:03fe5e16a33c 908 //Matrix K = (Phat*MatrixMath::Transpose(H))*(MatrixMath::Inv(MatrixMath::Transpose(A)*A+10.0f*MatrixMath::Eye(3))*MatrixMath::Transpose(A));
NaotoMorita 59:03fe5e16a33c 909 Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
NaotoMorita 59:03fe5e16a33c 910 Matrix z(3,1);
NaotoMorita 59:03fe5e16a33c 911 z << velgps(1,1) - vihat(1,1) << velgps(2,1)-vihat(2,1) << zmag(2,1);
NaotoMorita 59:03fe5e16a33c 912 errState = K * z;
NaotoMorita 59:03fe5e16a33c 913 Phat = (MatrixMath::Eye(nState)-K*H)*Phat;
NaotoMorita 59:03fe5e16a33c 914 //Phat = Phat-K*(H*Phat*MatrixMath::Transpose(H)+R)*MatrixMath::Transpose(K);
NaotoMorita 59:03fe5e16a33c 915 //Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*R*MatrixMath::Transpose(K);
NaotoMorita 59:03fe5e16a33c 916 fuseErr2Nominal();
NaotoMorita 59:03fe5e16a33c 917 }
NaotoMorita 56:c10f1168bd4a 918
NaotoMorita 59:03fe5e16a33c 919 void solaESKF::updateGPSPosition(Matrix posgps,float palt,Matrix R)
NaotoMorita 59:03fe5e16a33c 920 {
NaotoMorita 59:03fe5e16a33c 921 Matrix H(3,nState);
NaotoMorita 59:03fe5e16a33c 922 H(1,1) = 1.0f;
NaotoMorita 59:03fe5e16a33c 923 H(2,2) = 1.0f;
NaotoMorita 59:03fe5e16a33c 924 H(3,3) = 1.0f;
NaotoMorita 59:03fe5e16a33c 925
NaotoMorita 59:03fe5e16a33c 926 //Matrix A = H*Phat*MatrixMath::Transpose(H)+R;
NaotoMorita 59:03fe5e16a33c 927 //Matrix K = (Phat*MatrixMath::Transpose(H))*(MatrixMath::Inv(MatrixMath::Transpose(A)*A+1000.0f*MatrixMath::Eye(3))*MatrixMath::Transpose(A));
NaotoMorita 59:03fe5e16a33c 928 Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
NaotoMorita 59:03fe5e16a33c 929 Matrix z(3,1);
NaotoMorita 59:03fe5e16a33c 930 z << posgps(1,1) - pihat(1,1) << posgps(2,1)-pihat(2,1) << palt - pihat(3,1);
NaotoMorita 59:03fe5e16a33c 931 errState = K * z;
NaotoMorita 59:03fe5e16a33c 932 Phat = (MatrixMath::Eye(nState)-K*H)*Phat;
NaotoMorita 59:03fe5e16a33c 933 //Phat = Phat-K*(H*Phat*MatrixMath::Transpose(H)+R)*MatrixMath::Transpose(K);
NaotoMorita 59:03fe5e16a33c 934 //Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*R*MatrixMath::Transpose(K);
NaotoMorita 59:03fe5e16a33c 935 fuseErr2Nominal();
NaotoMorita 59:03fe5e16a33c 936 }
NaotoMorita 61:5e5c4fe12440 937
NaotoMorita 61:5e5c4fe12440 938 void solaESKF::updateImuConstraints(Matrix acc,Matrix mag,Matrix R)
NaotoMorita 61:5e5c4fe12440 939 {
NaotoMorita 61:5e5c4fe12440 940 Matrix accm = acc - accBias;
NaotoMorita 61:5e5c4fe12440 941 Matrix magm = mag - magBias;
NaotoMorita 61:5e5c4fe12440 942 Matrix dcm(3,3);
NaotoMorita 61:5e5c4fe12440 943 computeDcm(dcm, qhat);
NaotoMorita 61:5e5c4fe12440 944 Matrix tdcm = MatrixMath::Transpose(dcm);
NaotoMorita 61:5e5c4fe12440 945 Matrix tdcm_g = tdcm*gravity;
NaotoMorita 61:5e5c4fe12440 946 Matrix rotgrav = MatrixMath::Matrixcross(tdcm_g(1,1),tdcm_g(2,1),tdcm_g(3,1));
NaotoMorita 61:5e5c4fe12440 947 Matrix rotmag = -dcm*MatrixMath::Matrixcross(magm(1,1),magm(2,1),magm(3,1));
NaotoMorita 61:5e5c4fe12440 948
NaotoMorita 61:5e5c4fe12440 949 Matrix H(5,nState);
NaotoMorita 61:5e5c4fe12440 950 for (int i = 1; i < 4; i++){
NaotoMorita 61:5e5c4fe12440 951 for (int j = 1; j < 4; j++){
NaotoMorita 61:5e5c4fe12440 952 H(i,j+6) = rotgrav(i,j);
NaotoMorita 61:5e5c4fe12440 953 }
NaotoMorita 61:5e5c4fe12440 954 H(i,16) = tdcm(i,3);
NaotoMorita 61:5e5c4fe12440 955 }
NaotoMorita 61:5e5c4fe12440 956
NaotoMorita 61:5e5c4fe12440 957 H(1,10) = -1.0f;
NaotoMorita 61:5e5c4fe12440 958 H(2,11) = -1.0f;
NaotoMorita 61:5e5c4fe12440 959 H(3,12) = -1.0f;
NaotoMorita 61:5e5c4fe12440 960
NaotoMorita 61:5e5c4fe12440 961 Matrix magned = dcm*magm;
NaotoMorita 61:5e5c4fe12440 962 float hx = sqrt(magned(1,1)*magned(1,1)+magned(2,1)*magned(2,1));
NaotoMorita 61:5e5c4fe12440 963
NaotoMorita 61:5e5c4fe12440 964 for(int j = 1; j < 4; j++){
NaotoMorita 61:5e5c4fe12440 965 H(4,j+6) = rotmag(1,j)-(rotmag(1,j)+rotmag(2,j))/hx;
NaotoMorita 61:5e5c4fe12440 966 H(4,j+16) = -dcm(1,j)+(dcm(1,j)+dcm(2,j))/hx;
NaotoMorita 61:5e5c4fe12440 967 H(5,j+6) = rotmag(2,j);
NaotoMorita 61:5e5c4fe12440 968 H(5,j+16) = -dcm(2,j);
NaotoMorita 61:5e5c4fe12440 969 }
NaotoMorita 61:5e5c4fe12440 970
NaotoMorita 61:5e5c4fe12440 971 Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
NaotoMorita 61:5e5c4fe12440 972 Matrix zacc = -accm-tdcm*gravity;
NaotoMorita 61:5e5c4fe12440 973 Matrix zmag = dcm*magm;
NaotoMorita 61:5e5c4fe12440 974 Matrix z(5,1);
NaotoMorita 61:5e5c4fe12440 975 z << zacc(1,1) << zacc(2,1) << zacc(3,1) << -(zmag(1,1) - hx) << -zmag(2,1);
NaotoMorita 61:5e5c4fe12440 976 twelite.printf("%f %f\r\n",hx,(zmag(1,1) - hx));
NaotoMorita 61:5e5c4fe12440 977 errState = K * z;
NaotoMorita 61:5e5c4fe12440 978 Phat = (MatrixMath::Eye(nState)-K*H)*Phat;
NaotoMorita 61:5e5c4fe12440 979
NaotoMorita 61:5e5c4fe12440 980 fuseErr2Nominal();
NaotoMorita 61:5e5c4fe12440 981 }
NaotoMorita 73:5770a0d470c0 982 float q0 = qhat(1,1);
NaotoMorita 73:5770a0d470c0 983 float q1 = qhat(2,1);
NaotoMorita 73:5770a0d470c0 984 float q2 = qhat(3,1);
NaotoMorita 73:5770a0d470c0 985 float q3 = qhat(4,1);
NaotoMorita 73:5770a0d470c0 986
NaotoMorita 73:5770a0d470c0 987 float d0 = (-q3*q3-q2*q2+q1*q1+q0*q0);
NaotoMorita 73:5770a0d470c0 988 float q0q3q1q2 = (q0*q3+q1*q2);
NaotoMorita 73:5770a0d470c0 989 float h1lower = 2.0f*(4.0f*q0q3q1q2*q0q3q1q2/d0/d0+1.0f)*sqrt(4.0f*q0q3q1q2*q0q3q1q2/d0/d0+1.0f);
NaotoMorita 73:5770a0d470c0 990
NaotoMorita 73:5770a0d470c0 991 float d1 = d0*sqrt(4.0f*q0q3q1q2*q0q3q1q2/d0/d0+1.0f);
NaotoMorita 73:5770a0d470c0 992 float d2 = d0*d0*sqrt(4.0f*q0q3q1q2*q0q3q1q2/d0/d0+1.0f);
NaotoMorita 73:5770a0d470c0 993 float d3 = d0*sqrt(4.0f*q0q3q1q2*q0q3q1q2/d0/d0+1.0f)*(4.0f*q0q3q1q2*q0q3q1q2/d0/d0+1.0f);
NaotoMorita 73:5770a0d470c0 994
NaotoMorita 73:5770a0d470c0 995
NaotoMorita 73:5770a0d470c0 996
NaotoMorita 73:5770a0d470c0 997 Matrix Hh(2,4);
NaotoMorita 73:5770a0d470c0 998 Hh(1,1) = -(8.0f*q3*q0q3q1q2/d0/d0-16.0f*q0*q0q3q1q2*q0q3q1q2/d0/d0/d0)/h1lower;
NaotoMorita 73:5770a0d470c0 999 Hh(1,2) = -(8.0f*q2*q0q3q1q2/d0/d0-16.0f*q1*q0q3q1q2*q0q3q1q2/d0/d0/d0)/h1lower;
NaotoMorita 73:5770a0d470c0 1000 Hh(1,3) = -(8.0f*q1*q0q3q1q2/d0/d0-16.0f*q2*q0q3q1q2*q0q3q1q2/d0/d0/d0)/h1lower;
NaotoMorita 73:5770a0d470c0 1001 Hh(1,4) = -(8.0f*q0*q0q3q1q2/d0/d0-16.0f*q3*q0q3q1q2*q0q3q1q2/d0/d0/d0)/h1lower;
NaotoMorita 73:5770a0d470c0 1002
NaotoMorita 73:5770a0d470c0 1003 Hh(2,1) = 2.0f*q3/d1-4.0f*q0*q0q3q1q2/d2-q0q3q1q2*(8.0f*q3*q0q3q1q2/d0/d0-16.0f*q0*q0q3q1q2*q0q3q1q2/d0/d0/d0)/d3;
NaotoMorita 73:5770a0d470c0 1004 Hh(2,2) = 2.0f*q2/d1-4.0f*q1*q0q3q1q2/d2-q0q3q1q2*(8.0f*q2*q0q3q1q2/d0/d0-16.0f*q1*q0q3q1q2*q0q3q1q2/d0/d0/d0)/d3;
NaotoMorita 73:5770a0d470c0 1005 Hh(2,3) = 2.0f*q1/d1+4.0f*q2*q0q3q1q2/d2-q0q3q1q2*(8.0f*q1*q0q3q1q2/d0/d0-16.0f*q2*q0q3q1q2*q0q3q1q2/d0/d0/d0)/d3;
NaotoMorita 73:5770a0d470c0 1006 Hh(2,4) = 2.0f*q0/d1+4.0f*q3*q0q3q1q2/d2-q0q3q1q2*(8.0f*q0*q0q3q1q2/d0/d0-16.0f*q3*q0q3q1q2*q0q3q1q2/d0/d0/d0)/d3;
NaotoMorita 56:c10f1168bd4a 1007 */