HAPSRG / solaESKF_Eigen

Eigen

Dependencies:   Eigen

Dependents:   optWingforHAPS_Eigen hexaTest_Eigen

solaESKF.cpp@83:8f6ae61d47ac, 2022-06-29 (annotated)

Committer:
NaotoMorita
Date:
Wed Jun 29 07:56:42 2022 +0000
Revision:
83:8f6ae61d47ac
Parent:
82:662b810155e9
Child:
84:fb14acabefd3
modifiy

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