HAPSRG / solaESKF_Eigen

Eigen

Dependencies:   Eigen

Dependents:   optWingforHAPS_Eigen hexaTest_Eigen

solaESKF.cpp@84:fb14acabefd3, 23 months ago (annotated)

Committer:
NaotoMorita
Date:
Wed Jul 13 05:08:01 2022 +0000
Revision:
84:fb14acabefd3
Parent:
83:8f6ae61d47ac
Child:
85:552fe30e7f51
Child:
86:cc2c0025d820
eigen2

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
NaotoMorita 84:fb14acabefd3 559 void solaESKF::fuseErr2Nominal(VectorXf errVal)
NaotoMorita 84:fb14acabefd3 560 {
NaotoMorita 84:fb14acabefd3 561 //position
NaotoMorita 84:fb14acabefd3 562 pihat(0) += errVal(0);
NaotoMorita 84:fb14acabefd3 563 pihat(1) += errVal(1);
NaotoMorita 84:fb14acabefd3 564 pihat(2) += errVal(2);
NaotoMorita 84:fb14acabefd3 565
NaotoMorita 84:fb14acabefd3 566 //velocity
NaotoMorita 84:fb14acabefd3 567 vihat(0) += errVal(3);
NaotoMorita 84:fb14acabefd3 568 vihat(1) += errVal(4);
NaotoMorita 84:fb14acabefd3 569 vihat(2) += errVal(5);
NaotoMorita 84:fb14acabefd3 570
NaotoMorita 84:fb14acabefd3 571 //angle error
NaotoMorita 84:fb14acabefd3 572 Vector4f qerr;
NaotoMorita 84:fb14acabefd3 573 qerr << 1.0f, 0.5f*errVal(6), 0.5f*errVal(7), 0.5f*errVal(8);
NaotoMorita 84:fb14acabefd3 574 qhat = quatmultiply(qhat, qerr);
NaotoMorita 84:fb14acabefd3 575 qhat.normalize();
NaotoMorita 84:fb14acabefd3 576
NaotoMorita 84:fb14acabefd3 577 //acc bias
NaotoMorita 84:fb14acabefd3 578 accBias(0) += errVal(9);
NaotoMorita 84:fb14acabefd3 579 accBias(1) += errVal(10);
NaotoMorita 84:fb14acabefd3 580 accBias(2) += errVal(11);
NaotoMorita 84:fb14acabefd3 581
NaotoMorita 84:fb14acabefd3 582 //gyro bias
NaotoMorita 84:fb14acabefd3 583 gyroBias(0) += errVal(12);
NaotoMorita 84:fb14acabefd3 584 gyroBias(1) += errVal(13);
NaotoMorita 84:fb14acabefd3 585 gyroBias(2) += errVal(14);
NaotoMorita 84:fb14acabefd3 586
NaotoMorita 84:fb14acabefd3 587 //gravity bias
NaotoMorita 84:fb14acabefd3 588 //gravity(0) += errVal(15);
NaotoMorita 84:fb14acabefd3 589 //gravity(1) += errVal(16);
NaotoMorita 84:fb14acabefd3 590 //gravity(2) += errVal(17);
NaotoMorita 84:fb14acabefd3 591 }
NaotoMorita 84:fb14acabefd3 592
cocorlow 78:e36a7b844fb5 593 Vector4f solaESKF::quatmultiply(Vector4f p, Vector4f q)
NaotoMorita 19:3fae66745363 594 {
cocorlow 78:e36a7b844fb5 595 Vector4f qout;
cocorlow 78:e36a7b844fb5 596 qout(0) = p(0)*q(0) - p(1)*q(1) - p(2)*q(2) - p(3)*q(3);
cocorlow 78:e36a7b844fb5 597 qout(1) = p(0)*q(1) + p(1)*q(0) + p(2)*q(3) - p(3)*q(2);
cocorlow 78:e36a7b844fb5 598 qout(2) = p(0)*q(2) - p(1)*q(3) + p(2)*q(0) + p(3)*q(1);
cocorlow 78:e36a7b844fb5 599 qout(3) = p(0)*q(3) + p(1)*q(2) - p(2)*q(1) + p(3)*q(0);
cocorlow 78:e36a7b844fb5 600 // qout.normalize();
NaotoMorita 19:3fae66745363 601 return qout;
NaotoMorita 19:3fae66745363 602 }
NaotoMorita 19:3fae66745363 603
cocorlow 78:e36a7b844fb5 604 void solaESKF::computeDcm(Matrix3f& dcm, Vector4f quat)
NaotoMorita 19:3fae66745363 605 {
cocorlow 78:e36a7b844fb5 606 // 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 607 // dcm(1,2) = 2.0f*(quat(2,1)*quat(3,1) - quat(1,1)*quat(4,1));
cocorlow 78:e36a7b844fb5 608 // dcm(1,3) = 2.0f*(quat(2,1)*quat(4,1) + quat(1,1)*quat(3,1));
cocorlow 78:e36a7b844fb5 609 // dcm(2,1) = 2.0f*(quat(2,1)*quat(3,1) + quat(1,1)*quat(4,1));
cocorlow 78:e36a7b844fb5 610 // 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 611 // dcm(2,3) = 2.0f*(quat(3,1)*quat(4,1) - quat(1,1)*quat(2,1));
cocorlow 78:e36a7b844fb5 612 // dcm(3,1) = 2.0f*(quat(2,1)*quat(4,1) - quat(1,1)*quat(3,1));
cocorlow 78:e36a7b844fb5 613 // dcm(3,2) = 2.0f*(quat(3,1)*quat(4,1) + quat(1,1)*quat(2,1));
cocorlow 78:e36a7b844fb5 614 // 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 615
cocorlow 78:e36a7b844fb5 616 dcm(0,0) = quat(0)*quat(0) + quat(1)*quat(1) - quat(2)*quat(2) - quat(3)*quat(3);
cocorlow 78:e36a7b844fb5 617 dcm(0,1) = 2.0f*(quat(1)*quat(2) - quat(0)*quat(3));
cocorlow 78:e36a7b844fb5 618 dcm(0,2) = 2.0f*(quat(1)*quat(3) + quat(0)*quat(2));
cocorlow 78:e36a7b844fb5 619 dcm(1,0) = 2.0f*(quat(1)*quat(2) + quat(0)*quat(3));
cocorlow 78:e36a7b844fb5 620 dcm(1,1) = quat(0)*quat(0) - quat(1)*quat(1) + quat(2)*quat(2) - quat(3)*quat(3);
cocorlow 78:e36a7b844fb5 621 dcm(1,2) = 2.0f*(quat(2)*quat(3) - quat(0)*quat(1));
cocorlow 78:e36a7b844fb5 622 dcm(2,0) = 2.0f*(quat(1)*quat(3) - quat(0)*quat(2));
cocorlow 78:e36a7b844fb5 623 dcm(2,1) = 2.0f*(quat(2)*quat(3) + quat(0)*quat(1));
cocorlow 78:e36a7b844fb5 624 dcm(2,2) = quat(0)*quat(0) - quat(1)*quat(1) - quat(2)*quat(2) + quat(3)*quat(3);
NaotoMorita 19:3fae66745363 625 }
NaotoMorita 19:3fae66745363 626
NaotoMorita 44:7d82e63b6a86 627 void solaESKF::setQhat(float ex,float ey,float ez)
NaotoMorita 44:7d82e63b6a86 628 {
cocorlow 78:e36a7b844fb5 629 float cos_z_2 = std::cos(0.5f*ez);
cocorlow 78:e36a7b844fb5 630 float cos_y_2 = std::cos(0.5f*ey);
cocorlow 78:e36a7b844fb5 631 float cos_x_2 = std::cos(0.5f*ex);
NaotoMorita 19:3fae66745363 632
cocorlow 78:e36a7b844fb5 633 float sin_z_2 = std::sin(0.5f*ez);
cocorlow 78:e36a7b844fb5 634 float sin_y_2 = std::sin(0.5f*ey);
cocorlow 78:e36a7b844fb5 635 float sin_x_2 = std::sin(0.5f*ex);
NaotoMorita 19:3fae66745363 636
NaotoMorita 19:3fae66745363 637 // and now compute quaternion
cocorlow 78:e36a7b844fb5 638 qhat(0) = cos_z_2*cos_y_2*cos_x_2 + sin_z_2*sin_y_2*sin_x_2;
cocorlow 78:e36a7b844fb5 639 qhat(1) = cos_z_2*cos_y_2*sin_x_2 - sin_z_2*sin_y_2*cos_x_2;
cocorlow 78:e36a7b844fb5 640 qhat(2) = cos_z_2*sin_y_2*cos_x_2 + sin_z_2*cos_y_2*sin_x_2;
cocorlow 78:e36a7b844fb5 641 qhat(3) = sin_z_2*cos_y_2*cos_x_2 - cos_z_2*sin_y_2*sin_x_2;
NaotoMorita 19:3fae66745363 642 }
NaotoMorita 19:3fae66745363 643
cocorlow 78:e36a7b844fb5 644 Vector3f solaESKF::calcDynAcc(Vector3f acc)
NaotoMorita 75:e2c825cdc511 645 {
cocorlow 78:e36a7b844fb5 646 Vector3f accm = acc - accBias;
NaotoMorita 80:b241c058df83 647 Matrix3f dcm;
NaotoMorita 80:b241c058df83 648 computeDcm(dcm, qhat);
NaotoMorita 80:b241c058df83 649 Matrix3f tdcm = dcm.transpose();
NaotoMorita 75:e2c825cdc511 650
cocorlow 78:e36a7b844fb5 651 Vector3f dynAcc = accm+tdcm*gravity;
NaotoMorita 75:e2c825cdc511 652 return dynAcc;
NaotoMorita 75:e2c825cdc511 653 }
NaotoMorita 47:2467de40951f 654
NaotoMorita 83:8f6ae61d47ac 655 Vector3f solaESKF::calcVb()
NaotoMorita 82:662b810155e9 656 {
NaotoMorita 82:662b810155e9 657 Matrix3f dcm;
NaotoMorita 82:662b810155e9 658 computeDcm(dcm, qhat);
NaotoMorita 82:662b810155e9 659 Matrix3f tdcm = dcm.transpose();
NaotoMorita 82:662b810155e9 660
NaotoMorita 82:662b810155e9 661 Vector3f Vb = tdcm*vihat;
NaotoMorita 83:8f6ae61d47ac 662 return Vb;
NaotoMorita 82:662b810155e9 663 }
NaotoMorita 82:662b810155e9 664
NaotoMorita 47:2467de40951f 665
NaotoMorita 44:7d82e63b6a86 666 void solaESKF::setGravity(float gx,float gy,float gz)
NaotoMorita 23:1509648c2318 667 {
cocorlow 78:e36a7b844fb5 668 gravity(0) = gx;
cocorlow 78:e36a7b844fb5 669 gravity(1) = gy;
cocorlow 78:e36a7b844fb5 670 gravity(2) = gz;
NaotoMorita 23:1509648c2318 671 }
NaotoMorita 62:5519d34eb6e8 672
cocorlow 78:e36a7b844fb5 673 Vector3f solaESKF::getPihat()
NaotoMorita 47:2467de40951f 674 {
NaotoMorita 47:2467de40951f 675 return pihat;
NaotoMorita 47:2467de40951f 676 }
cocorlow 78:e36a7b844fb5 677 Vector3f solaESKF::getVihat()
NaotoMorita 47:2467de40951f 678 {
NaotoMorita 47:2467de40951f 679 return vihat;
NaotoMorita 47:2467de40951f 680 }
cocorlow 78:e36a7b844fb5 681 Vector4f solaESKF::getQhat()
NaotoMorita 47:2467de40951f 682 {
NaotoMorita 47:2467de40951f 683 return qhat;
NaotoMorita 47:2467de40951f 684 }
cocorlow 78:e36a7b844fb5 685 Vector3f solaESKF::getAccBias()
NaotoMorita 47:2467de40951f 686 {
NaotoMorita 47:2467de40951f 687 return accBias;
NaotoMorita 47:2467de40951f 688 }
cocorlow 78:e36a7b844fb5 689 Vector3f solaESKF::getGyroBias()
NaotoMorita 47:2467de40951f 690 {
NaotoMorita 47:2467de40951f 691 return gyroBias;
NaotoMorita 47:2467de40951f 692 }
cocorlow 78:e36a7b844fb5 693 Vector3f solaESKF::getGravity()
NaotoMorita 47:2467de40951f 694 {
NaotoMorita 47:2467de40951f 695 return gravity;
NaotoMorita 47:2467de40951f 696 }
NaotoMorita 74:f5fe7fecbd3c 697
cocorlow 78:e36a7b844fb5 698 VectorXf solaESKF::getErrState()
NaotoMorita 47:2467de40951f 699 {
NaotoMorita 47:2467de40951f 700 return errState;
NaotoMorita 47:2467de40951f 701 }
NaotoMorita 23:1509648c2318 702
NaotoMorita 80:b241c058df83 703 VectorXf solaESKF::getState()
NaotoMorita 47:2467de40951f 704 {
NaotoMorita 80:b241c058df83 705 VectorXf state = VectorXf::Zero(nState+1);
NaotoMorita 80:b241c058df83 706 for (int i = 0; i < 3; i++){
NaotoMorita 80:b241c058df83 707 state(i) = pihat(i);
NaotoMorita 80:b241c058df83 708 state(i+3) = vihat(i);
NaotoMorita 80:b241c058df83 709 }
NaotoMorita 80:b241c058df83 710 for (int i = 0; i < 4; i++){
NaotoMorita 80:b241c058df83 711 state(i+6) = qhat(i);
NaotoMorita 80:b241c058df83 712 }
NaotoMorita 80:b241c058df83 713 for (int i = 0; i < 3; i++){
NaotoMorita 80:b241c058df83 714 state(i+10) = accBias(i);
NaotoMorita 80:b241c058df83 715 state(i+13) = gyroBias(i);
NaotoMorita 80:b241c058df83 716 state(i+16) = gravity(i);
NaotoMorita 80:b241c058df83 717 }
NaotoMorita 80:b241c058df83 718 return state;
NaotoMorita 47:2467de40951f 719 }
NaotoMorita 80:b241c058df83 720
NaotoMorita 80:b241c058df83 721 VectorXf solaESKF::getVariance()
NaotoMorita 47:2467de40951f 722 {
NaotoMorita 80:b241c058df83 723 VectorXf variance = VectorXf::Zero(nState);
NaotoMorita 80:b241c058df83 724 for (int i = 0; i < nState; i++){
NaotoMorita 80:b241c058df83 725 variance(i) = Phat(i,i);
NaotoMorita 80:b241c058df83 726 }
NaotoMorita 80:b241c058df83 727 return variance;
NaotoMorita 80:b241c058df83 728 }
NaotoMorita 80:b241c058df83 729
NaotoMorita 80:b241c058df83 730 void solaESKF::setPhatPosition(float valNE,float valD)
NaotoMorita 80:b241c058df83 731 {
NaotoMorita 80:b241c058df83 732 setBlockDiag(Phat, valNE, 0,2);
NaotoMorita 80:b241c058df83 733 Phat(2,2) = valD;
NaotoMorita 80:b241c058df83 734 }
NaotoMorita 80:b241c058df83 735 void solaESKF::setPhatVelocity(float valNE,float valD)
NaotoMorita 80:b241c058df83 736 {
NaotoMorita 80:b241c058df83 737 setBlockDiag(Phat, valNE, 3,5);
NaotoMorita 80:b241c058df83 738 Phat(5,5) = valD;
NaotoMorita 47:2467de40951f 739 }
NaotoMorita 47:2467de40951f 740 void solaESKF::setPhatAngleError(float val)
NaotoMorita 47:2467de40951f 741 {
cocorlow 78:e36a7b844fb5 742 setBlockDiag(Phat, val, 6,8);
NaotoMorita 47:2467de40951f 743 }
NaotoMorita 47:2467de40951f 744 void solaESKF::setPhatAccBias(float val)
NaotoMorita 47:2467de40951f 745 {
cocorlow 78:e36a7b844fb5 746 setBlockDiag(Phat, val, 9,11);
NaotoMorita 47:2467de40951f 747 }
NaotoMorita 47:2467de40951f 748 void solaESKF::setPhatGyroBias(float val)
NaotoMorita 47:2467de40951f 749 {
cocorlow 78:e36a7b844fb5 750 setBlockDiag(Phat, val, 12,14);
NaotoMorita 47:2467de40951f 751 }
NaotoMorita 47:2467de40951f 752 void solaESKF::setPhatGravity(float val)
NaotoMorita 47:2467de40951f 753 {
cocorlow 78:e36a7b844fb5 754 setBlockDiag(Phat, val, 15,17);
NaotoMorita 62:5519d34eb6e8 755 }
NaotoMorita 47:2467de40951f 756
NaotoMorita 47:2467de40951f 757
NaotoMorita 80:b241c058df83 758 void solaESKF::setQVelocity(float valNE,float valD)
NaotoMorita 47:2467de40951f 759 {
NaotoMorita 80:b241c058df83 760 setBlockDiag(Q, valNE, 3, 5);
NaotoMorita 80:b241c058df83 761 Q(5,5) = valD;
NaotoMorita 47:2467de40951f 762 }
NaotoMorita 47:2467de40951f 763 void solaESKF::setQAngleError(float val)
NaotoMorita 47:2467de40951f 764 {
cocorlow 78:e36a7b844fb5 765 setBlockDiag(Q, val, 6, 8);
NaotoMorita 47:2467de40951f 766 }
NaotoMorita 47:2467de40951f 767 void solaESKF::setQAccBias(float val)
NaotoMorita 47:2467de40951f 768 {
cocorlow 78:e36a7b844fb5 769 setBlockDiag(Q, val, 9, 11);
NaotoMorita 47:2467de40951f 770 }
NaotoMorita 47:2467de40951f 771 void solaESKF::setQGyroBias(float val)
NaotoMorita 47:2467de40951f 772 {
cocorlow 78:e36a7b844fb5 773 setBlockDiag(Q, val, 12, 14);
NaotoMorita 47:2467de40951f 774 }
NaotoMorita 19:3fae66745363 775
NaotoMorita 80:b241c058df83 776 void solaESKF::setDiag(Matrix3f& mat, float val){
NaotoMorita 80:b241c058df83 777 for (int i = 0; i < mat.cols(); i++){
NaotoMorita 80:b241c058df83 778 for (int j = 0; j < mat.rows(); j++){
NaotoMorita 80:b241c058df83 779 mat(i,j) = 0.0f;
NaotoMorita 80:b241c058df83 780 }
NaotoMorita 80:b241c058df83 781 }
cocorlow 78:e36a7b844fb5 782 for (int i = 0; i < mat.cols(); i++){
NaotoMorita 44:7d82e63b6a86 783 mat(i,i) = val;
NaotoMorita 22:7d84b8bc20b4 784 }
NaotoMorita 22:7d84b8bc20b4 785 }
NaotoMorita 22:7d84b8bc20b4 786
NaotoMorita 80:b241c058df83 787 void solaESKF::setDiag(MatrixXf& mat, float val){
NaotoMorita 80:b241c058df83 788 for (int i = 0; i < mat.cols(); i++){
NaotoMorita 80:b241c058df83 789 for (int j = 0; j < mat.rows(); j++){
NaotoMorita 80:b241c058df83 790 mat(i,j) = 0.0f;
NaotoMorita 80:b241c058df83 791 }
NaotoMorita 80:b241c058df83 792 }
NaotoMorita 80:b241c058df83 793 for (int i = 0; i < mat.cols(); i++)
NaotoMorita 80:b241c058df83 794 {
NaotoMorita 80:b241c058df83 795 mat(i, i) = val;
NaotoMorita 80:b241c058df83 796 }
NaotoMorita 80:b241c058df83 797 }
NaotoMorita 80:b241c058df83 798
cocorlow 78:e36a7b844fb5 799 void solaESKF::setBlockDiag(MatrixXf& mat, float val, int startIndex, int endIndex){
NaotoMorita 80:b241c058df83 800
NaotoMorita 44:7d82e63b6a86 801 for (int i = startIndex; i < endIndex+1; i++){
NaotoMorita 44:7d82e63b6a86 802 mat(i,i) = val;
NaotoMorita 22:7d84b8bc20b4 803 }
NaotoMorita 22:7d84b8bc20b4 804 }
osaka 51:a5af3b280d23 805
NaotoMorita 81:230a3d2b0493 806 void solaESKF::setPihat(float pi_x, float pi_y, float pi_z)
osaka 51:a5af3b280d23 807 {
cocorlow 78:e36a7b844fb5 808 pihat(0) = pi_x;
cocorlow 78:e36a7b844fb5 809 pihat(1) = pi_y;
NaotoMorita 82:662b810155e9 810 pihat(2) = pi_z;
NaotoMorita 56:c10f1168bd4a 811 }
NaotoMorita 73:5770a0d470c0 812
cocorlow 78:e36a7b844fb5 813 Matrix3f solaESKF::Matrixcross(Vector3f v)
cocorlow 78:e36a7b844fb5 814 {
NaotoMorita 83:8f6ae61d47ac 815 Matrix3f m = Matrix3f::Zero();
cocorlow 78:e36a7b844fb5 816 m << 0.0f, -v(2), v(1),
cocorlow 78:e36a7b844fb5 817 v(2), 0.0f, -v(0),
cocorlow 78:e36a7b844fb5 818 -v(1), v(0), 0.0f;
cocorlow 78:e36a7b844fb5 819 return m;
cocorlow 78:e36a7b844fb5 820 }
NaotoMorita 56:c10f1168bd4a 821 /*
NaotoMorita 56:c10f1168bd4a 822 void solaESKF::updateAccConstraints(Matrix acc,float palt,Matrix R)
NaotoMorita 56:c10f1168bd4a 823 {
NaotoMorita 56:c10f1168bd4a 824 Matrix accm = acc - accBias;
NaotoMorita 56:c10f1168bd4a 825 Matrix tdcm(3,3);
NaotoMorita 56:c10f1168bd4a 826 computeDcm(tdcm, qhat);
NaotoMorita 56:c10f1168bd4a 827 tdcm = MatrixMath::Transpose(tdcm);
NaotoMorita 56:c10f1168bd4a 828 Matrix tdcm_g = tdcm*gravity;
NaotoMorita 56:c10f1168bd4a 829 Matrix a2v = MatrixMath::Matrixcross(tdcm_g(1,1),tdcm_g(2,1),tdcm_g(3,1));
NaotoMorita 56:c10f1168bd4a 830
NaotoMorita 56:c10f1168bd4a 831 Matrix H(4,nState);
NaotoMorita 56:c10f1168bd4a 832 for (int i = 1; i < 4; i++){
NaotoMorita 56:c10f1168bd4a 833 for (int j = 1; j < 4; j++){
NaotoMorita 56:c10f1168bd4a 834 H(i,j+6) = a2v(i,j);
NaotoMorita 56:c10f1168bd4a 835 H(i,j+15) = tdcm(i,j);
NaotoMorita 56:c10f1168bd4a 836 }
NaotoMorita 56:c10f1168bd4a 837 }
NaotoMorita 56:c10f1168bd4a 838 H(1,10) = -1.0f;
NaotoMorita 56:c10f1168bd4a 839 H(2,11) = -1.0f;
NaotoMorita 56:c10f1168bd4a 840 H(3,12) = -1.0f;
NaotoMorita 56:c10f1168bd4a 841 H(4,3) = 1.0f;
NaotoMorita 56:c10f1168bd4a 842
NaotoMorita 56:c10f1168bd4a 843 //Matrix A = H*Phat*MatrixMath::Transpose(H)+R;
NaotoMorita 56:c10f1168bd4a 844 //Matrix K = (Phat*MatrixMath::Transpose(H))*(MatrixMath::Inv(MatrixMath::Transpose(A)*A+10.0f*MatrixMath::Eye(3))*MatrixMath::Transpose(A));
NaotoMorita 56:c10f1168bd4a 845 Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
NaotoMorita 56:c10f1168bd4a 846 Matrix zacc = -accm-tdcm*gravity;
NaotoMorita 56:c10f1168bd4a 847 Matrix z(4,1);
NaotoMorita 56:c10f1168bd4a 848 z << zacc(1,1) << zacc(2,1) << zacc(3,1) << palt - pihat(3,1);
NaotoMorita 56:c10f1168bd4a 849 errState = K * z;
NaotoMorita 56:c10f1168bd4a 850 Phat = (MatrixMath::Eye(nState)-K*H)*Phat;
NaotoMorita 56:c10f1168bd4a 851 //Phat = Phat-K*(H*Phat*MatrixMath::Transpose(H)+R)*MatrixMath::Transpose(K);
NaotoMorita 56:c10f1168bd4a 852 //Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*R*MatrixMath::Transpose(K);
NaotoMorita 56:c10f1168bd4a 853 fuseErr2Nominal();
NaotoMorita 56:c10f1168bd4a 854 }
NaotoMorita 56:c10f1168bd4a 855
NaotoMorita 56:c10f1168bd4a 856 void solaESKF::updateGyroConstraints(Matrix gyro,Matrix R)
NaotoMorita 56:c10f1168bd4a 857 {
NaotoMorita 56:c10f1168bd4a 858 Matrix gyrom = gyro - gyroBias;
NaotoMorita 56:c10f1168bd4a 859 Matrix dcm(3,3);
NaotoMorita 56:c10f1168bd4a 860 computeDcm(dcm, qhat);
NaotoMorita 56:c10f1168bd4a 861 Matrix a2v = dcm*MatrixMath::Matrixcross(gyrom(1,1),gyrom(2,1),gyrom(3,1));
NaotoMorita 56:c10f1168bd4a 862
NaotoMorita 56:c10f1168bd4a 863 Matrix H(2,nState);
NaotoMorita 56:c10f1168bd4a 864 for (int i = 1; i < 3; i++){
NaotoMorita 56:c10f1168bd4a 865 for (int j = 1; j < 4; j++){
NaotoMorita 56:c10f1168bd4a 866 H(i,j+6) = a2v(i,j);
NaotoMorita 56:c10f1168bd4a 867 H(i,j+12) = -dcm(i,j);
NaotoMorita 56:c10f1168bd4a 868 }
NaotoMorita 56:c10f1168bd4a 869 }
NaotoMorita 56:c10f1168bd4a 870 //Matrix A = H*Phat*MatrixMath::Transpose(H)+R;
NaotoMorita 56:c10f1168bd4a 871 //Matrix K = (Phat*MatrixMath::Transpose(H))*(MatrixMath::Inv(MatrixMath::Transpose(A)*A+10.0f*MatrixMath::Eye(2))*MatrixMath::Transpose(A));
NaotoMorita 56:c10f1168bd4a 872 Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
NaotoMorita 56:c10f1168bd4a 873
NaotoMorita 56:c10f1168bd4a 874 Matrix z3 = -dcm*gyrom;
NaotoMorita 56:c10f1168bd4a 875 Matrix z(2,1);
NaotoMorita 56:c10f1168bd4a 876 z << z3(1,1) << z3(2,1);
NaotoMorita 56:c10f1168bd4a 877 errState = K * z;
NaotoMorita 56:c10f1168bd4a 878 //Phat = (MatrixMath::Eye(nState)-K*H)*Phat;
NaotoMorita 56:c10f1168bd4a 879 //Phat = Phat-K*(H*Phat*MatrixMath::Transpose(H)+R)*MatrixMath::Transpose(K);
NaotoMorita 56:c10f1168bd4a 880 Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*R*MatrixMath::Transpose(K);
NaotoMorita 56:c10f1168bd4a 881 fuseErr2Nominal();
NaotoMorita 56:c10f1168bd4a 882 }
NaotoMorita 56:c10f1168bd4a 883 void solaESKF::updateMag(Matrix mag, Matrix R)
NaotoMorita 56:c10f1168bd4a 884 {
NaotoMorita 56:c10f1168bd4a 885 Matrix dcm(3,3);
NaotoMorita 56:c10f1168bd4a 886 computeDcm(dcm, qhat);
NaotoMorita 56:c10f1168bd4a 887 Matrix a2v = -dcm*MatrixMath::Matrixcross(mag(1,1),mag(2,1),mag(3,1));
NaotoMorita 56:c10f1168bd4a 888
NaotoMorita 56:c10f1168bd4a 889 Matrix H(2,nState);
NaotoMorita 56:c10f1168bd4a 890 for (int i = 1; i < 3; i++){
NaotoMorita 56:c10f1168bd4a 891 for (int j = 1; j < 4; j++){
NaotoMorita 56:c10f1168bd4a 892 H(i,j+6) = a2v(i,j);
NaotoMorita 56:c10f1168bd4a 893 }
NaotoMorita 56:c10f1168bd4a 894 }
NaotoMorita 56:c10f1168bd4a 895 H(1,19) = 1.0f;
NaotoMorita 56:c10f1168bd4a 896 //H(3,20) = 1.0f;
NaotoMorita 56:c10f1168bd4a 897
NaotoMorita 56:c10f1168bd4a 898 //Matrix A = H*Phat*MatrixMath::Transpose(H)+R;
NaotoMorita 56:c10f1168bd4a 899 //Matrix K = (Phat*MatrixMath::Transpose(H))*(MatrixMath::Inv(MatrixMath::Transpose(A)*A+10.0f*MatrixMath::Eye(3))*MatrixMath::Transpose(A));
NaotoMorita 56:c10f1168bd4a 900 Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
NaotoMorita 56:c10f1168bd4a 901 Matrix zmag = -dcm*mag-magField;
NaotoMorita 56:c10f1168bd4a 902 Matrix z(2,1);
NaotoMorita 56:c10f1168bd4a 903 z << zmag(1,1) << zmag(2,1);
NaotoMorita 56:c10f1168bd4a 904 errState = K * z;
NaotoMorita 56:c10f1168bd4a 905 Phat = (MatrixMath::Eye(nState)-K*H)*Phat;
NaotoMorita 56:c10f1168bd4a 906 //Phat = Phat-K*(H*Phat*MatrixMath::Transpose(H)+R)*MatrixMath::Transpose(K);
NaotoMorita 56:c10f1168bd4a 907 //Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*R*MatrixMath::Transpose(K);
NaotoMorita 56:c10f1168bd4a 908 fuseErr2Nominal();
NaotoMorita 56:c10f1168bd4a 909 }
NaotoMorita 59:03fe5e16a33c 910 void solaESKF::updateMag(Matrix mag,float palt, Matrix R)
NaotoMorita 59:03fe5e16a33c 911 {
NaotoMorita 59:03fe5e16a33c 912 Matrix dcm(3,3);
NaotoMorita 59:03fe5e16a33c 913 computeDcm(dcm, qhat);
NaotoMorita 59:03fe5e16a33c 914 Matrix a2v = -dcm*MatrixMath::Matrixcross(mag(1,1),mag(2,1),mag(3,1));
NaotoMorita 59:03fe5e16a33c 915
NaotoMorita 59:03fe5e16a33c 916 Matrix H(3,nState);
NaotoMorita 59:03fe5e16a33c 917 for (int i = 1; i < 3; i++){
NaotoMorita 59:03fe5e16a33c 918 for (int j = 1; j < 4; j++){
NaotoMorita 59:03fe5e16a33c 919 H(i,j+6) = a2v(i,j);
NaotoMorita 59:03fe5e16a33c 920 }
NaotoMorita 59:03fe5e16a33c 921 }
NaotoMorita 59:03fe5e16a33c 922 H(1,19) = 1.0f;
NaotoMorita 59:03fe5e16a33c 923 H(3,3) = 1.0f;
NaotoMorita 59:03fe5e16a33c 924 //Matrix A = H*Phat*MatrixMath::Transpose(H)+R;
NaotoMorita 59:03fe5e16a33c 925 //Matrix K = (Phat*MatrixMath::Transpose(H))*(MatrixMath::Inv(MatrixMath::Transpose(A)*A+10.0f*MatrixMath::Eye(3))*MatrixMath::Transpose(A));
NaotoMorita 59:03fe5e16a33c 926 Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
NaotoMorita 59:03fe5e16a33c 927 Matrix zmag = -dcm*mag-magField;
NaotoMorita 59:03fe5e16a33c 928 Matrix z(3,1);
NaotoMorita 59:03fe5e16a33c 929 z << zmag(1,1) << zmag(2,1) << palt - pihat(3,1);
NaotoMorita 59:03fe5e16a33c 930 errState = K * z;
NaotoMorita 59:03fe5e16a33c 931 Phat = (MatrixMath::Eye(nState)-K*H)*Phat;
NaotoMorita 59:03fe5e16a33c 932 //Phat = Phat-K*(H*Phat*MatrixMath::Transpose(H)+R)*MatrixMath::Transpose(K);
NaotoMorita 59:03fe5e16a33c 933 //Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*R*MatrixMath::Transpose(K);
NaotoMorita 59:03fe5e16a33c 934
NaotoMorita 59:03fe5e16a33c 935 fuseErr2Nominal();
NaotoMorita 59:03fe5e16a33c 936 }
NaotoMorita 59:03fe5e16a33c 937 void solaESKF::updateGPSVelocity(Matrix velgps,Matrix mag,Matrix R)
NaotoMorita 59:03fe5e16a33c 938 {
NaotoMorita 59:03fe5e16a33c 939
NaotoMorita 59:03fe5e16a33c 940 Matrix dcm(3,3);
NaotoMorita 59:03fe5e16a33c 941 computeDcm(dcm, qhat);
NaotoMorita 59:03fe5e16a33c 942 Matrix a2v = -dcm*MatrixMath::Matrixcross(mag(1,1),mag(2,1),mag(3,1));
NaotoMorita 59:03fe5e16a33c 943
NaotoMorita 59:03fe5e16a33c 944
NaotoMorita 59:03fe5e16a33c 945 Matrix H(3,nState);
NaotoMorita 59:03fe5e16a33c 946 H(1,4) = 1.0f;
NaotoMorita 59:03fe5e16a33c 947 H(2,5) = 1.0f;
NaotoMorita 59:03fe5e16a33c 948
NaotoMorita 59:03fe5e16a33c 949 for (int j = 1; j < 4; j++){
NaotoMorita 59:03fe5e16a33c 950 H(3,j+6) = a2v(2,j);
NaotoMorita 59:03fe5e16a33c 951 }
NaotoMorita 59:03fe5e16a33c 952 //H(3,19) = 1.0f;
NaotoMorita 59:03fe5e16a33c 953
NaotoMorita 59:03fe5e16a33c 954 Matrix zmag = -dcm*mag-magField;
NaotoMorita 59:03fe5e16a33c 955
NaotoMorita 59:03fe5e16a33c 956 //Matrix A = H*Phat*MatrixMath::Transpose(H)+R;
NaotoMorita 59:03fe5e16a33c 957 //Matrix K = (Phat*MatrixMath::Transpose(H))*(MatrixMath::Inv(MatrixMath::Transpose(A)*A+10.0f*MatrixMath::Eye(3))*MatrixMath::Transpose(A));
NaotoMorita 59:03fe5e16a33c 958 Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
NaotoMorita 59:03fe5e16a33c 959 Matrix z(3,1);
NaotoMorita 59:03fe5e16a33c 960 z << velgps(1,1) - vihat(1,1) << velgps(2,1)-vihat(2,1) << zmag(2,1);
NaotoMorita 59:03fe5e16a33c 961 errState = K * z;
NaotoMorita 59:03fe5e16a33c 962 Phat = (MatrixMath::Eye(nState)-K*H)*Phat;
NaotoMorita 59:03fe5e16a33c 963 //Phat = Phat-K*(H*Phat*MatrixMath::Transpose(H)+R)*MatrixMath::Transpose(K);
NaotoMorita 59:03fe5e16a33c 964 //Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*R*MatrixMath::Transpose(K);
NaotoMorita 59:03fe5e16a33c 965 fuseErr2Nominal();
NaotoMorita 59:03fe5e16a33c 966 }
NaotoMorita 56:c10f1168bd4a 967
NaotoMorita 59:03fe5e16a33c 968 void solaESKF::updateGPSPosition(Matrix posgps,float palt,Matrix R)
NaotoMorita 59:03fe5e16a33c 969 {
NaotoMorita 59:03fe5e16a33c 970 Matrix H(3,nState);
NaotoMorita 59:03fe5e16a33c 971 H(1,1) = 1.0f;
NaotoMorita 59:03fe5e16a33c 972 H(2,2) = 1.0f;
NaotoMorita 59:03fe5e16a33c 973 H(3,3) = 1.0f;
NaotoMorita 59:03fe5e16a33c 974
NaotoMorita 59:03fe5e16a33c 975 //Matrix A = H*Phat*MatrixMath::Transpose(H)+R;
NaotoMorita 59:03fe5e16a33c 976 //Matrix K = (Phat*MatrixMath::Transpose(H))*(MatrixMath::Inv(MatrixMath::Transpose(A)*A+1000.0f*MatrixMath::Eye(3))*MatrixMath::Transpose(A));
NaotoMorita 59:03fe5e16a33c 977 Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
NaotoMorita 59:03fe5e16a33c 978 Matrix z(3,1);
NaotoMorita 59:03fe5e16a33c 979 z << posgps(1,1) - pihat(1,1) << posgps(2,1)-pihat(2,1) << palt - pihat(3,1);
NaotoMorita 59:03fe5e16a33c 980 errState = K * z;
NaotoMorita 59:03fe5e16a33c 981 Phat = (MatrixMath::Eye(nState)-K*H)*Phat;
NaotoMorita 59:03fe5e16a33c 982 //Phat = Phat-K*(H*Phat*MatrixMath::Transpose(H)+R)*MatrixMath::Transpose(K);
NaotoMorita 59:03fe5e16a33c 983 //Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*R*MatrixMath::Transpose(K);
NaotoMorita 59:03fe5e16a33c 984 fuseErr2Nominal();
NaotoMorita 59:03fe5e16a33c 985 }
NaotoMorita 61:5e5c4fe12440 986
NaotoMorita 61:5e5c4fe12440 987 void solaESKF::updateImuConstraints(Matrix acc,Matrix mag,Matrix R)
NaotoMorita 61:5e5c4fe12440 988 {
NaotoMorita 61:5e5c4fe12440 989 Matrix accm = acc - accBias;
NaotoMorita 61:5e5c4fe12440 990 Matrix magm = mag - magBias;
NaotoMorita 61:5e5c4fe12440 991 Matrix dcm(3,3);
NaotoMorita 61:5e5c4fe12440 992 computeDcm(dcm, qhat);
NaotoMorita 61:5e5c4fe12440 993 Matrix tdcm = MatrixMath::Transpose(dcm);
NaotoMorita 61:5e5c4fe12440 994 Matrix tdcm_g = tdcm*gravity;
NaotoMorita 61:5e5c4fe12440 995 Matrix rotgrav = MatrixMath::Matrixcross(tdcm_g(1,1),tdcm_g(2,1),tdcm_g(3,1));
NaotoMorita 61:5e5c4fe12440 996 Matrix rotmag = -dcm*MatrixMath::Matrixcross(magm(1,1),magm(2,1),magm(3,1));
NaotoMorita 61:5e5c4fe12440 997
NaotoMorita 61:5e5c4fe12440 998 Matrix H(5,nState);
NaotoMorita 61:5e5c4fe12440 999 for (int i = 1; i < 4; i++){
NaotoMorita 61:5e5c4fe12440 1000 for (int j = 1; j < 4; j++){
NaotoMorita 61:5e5c4fe12440 1001 H(i,j+6) = rotgrav(i,j);
NaotoMorita 61:5e5c4fe12440 1002 }
NaotoMorita 61:5e5c4fe12440 1003 H(i,16) = tdcm(i,3);
NaotoMorita 61:5e5c4fe12440 1004 }
NaotoMorita 61:5e5c4fe12440 1005
NaotoMorita 61:5e5c4fe12440 1006 H(1,10) = -1.0f;
NaotoMorita 61:5e5c4fe12440 1007 H(2,11) = -1.0f;
NaotoMorita 61:5e5c4fe12440 1008 H(3,12) = -1.0f;
NaotoMorita 61:5e5c4fe12440 1009
NaotoMorita 61:5e5c4fe12440 1010 Matrix magned = dcm*magm;
NaotoMorita 61:5e5c4fe12440 1011 float hx = sqrt(magned(1,1)*magned(1,1)+magned(2,1)*magned(2,1));
NaotoMorita 61:5e5c4fe12440 1012
NaotoMorita 61:5e5c4fe12440 1013 for(int j = 1; j < 4; j++){
NaotoMorita 61:5e5c4fe12440 1014 H(4,j+6) = rotmag(1,j)-(rotmag(1,j)+rotmag(2,j))/hx;
NaotoMorita 61:5e5c4fe12440 1015 H(4,j+16) = -dcm(1,j)+(dcm(1,j)+dcm(2,j))/hx;
NaotoMorita 61:5e5c4fe12440 1016 H(5,j+6) = rotmag(2,j);
NaotoMorita 61:5e5c4fe12440 1017 H(5,j+16) = -dcm(2,j);
NaotoMorita 61:5e5c4fe12440 1018 }
NaotoMorita 61:5e5c4fe12440 1019
NaotoMorita 61:5e5c4fe12440 1020 Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
NaotoMorita 61:5e5c4fe12440 1021 Matrix zacc = -accm-tdcm*gravity;
NaotoMorita 61:5e5c4fe12440 1022 Matrix zmag = dcm*magm;
NaotoMorita 61:5e5c4fe12440 1023 Matrix z(5,1);
NaotoMorita 61:5e5c4fe12440 1024 z << zacc(1,1) << zacc(2,1) << zacc(3,1) << -(zmag(1,1) - hx) << -zmag(2,1);
NaotoMorita 61:5e5c4fe12440 1025 twelite.printf("%f %f\r\n",hx,(zmag(1,1) - hx));
NaotoMorita 61:5e5c4fe12440 1026 errState = K * z;
NaotoMorita 61:5e5c4fe12440 1027 Phat = (MatrixMath::Eye(nState)-K*H)*Phat;
NaotoMorita 61:5e5c4fe12440 1028
NaotoMorita 61:5e5c4fe12440 1029 fuseErr2Nominal();
NaotoMorita 61:5e5c4fe12440 1030 }
NaotoMorita 73:5770a0d470c0 1031 float q0 = qhat(1,1);
NaotoMorita 73:5770a0d470c0 1032 float q1 = qhat(2,1);
NaotoMorita 73:5770a0d470c0 1033 float q2 = qhat(3,1);
NaotoMorita 73:5770a0d470c0 1034 float q3 = qhat(4,1);
NaotoMorita 73:5770a0d470c0 1035
NaotoMorita 73:5770a0d470c0 1036 float d0 = (-q3*q3-q2*q2+q1*q1+q0*q0);
NaotoMorita 73:5770a0d470c0 1037 float q0q3q1q2 = (q0*q3+q1*q2);
NaotoMorita 73:5770a0d470c0 1038 float h1lower = 2.0f*(4.0f*q0q3q1q2*q0q3q1q2/d0/d0+1.0f)*sqrt(4.0f*q0q3q1q2*q0q3q1q2/d0/d0+1.0f);
NaotoMorita 73:5770a0d470c0 1039
NaotoMorita 73:5770a0d470c0 1040 float d1 = d0*sqrt(4.0f*q0q3q1q2*q0q3q1q2/d0/d0+1.0f);
NaotoMorita 73:5770a0d470c0 1041 float d2 = d0*d0*sqrt(4.0f*q0q3q1q2*q0q3q1q2/d0/d0+1.0f);
NaotoMorita 73:5770a0d470c0 1042 float d3 = d0*sqrt(4.0f*q0q3q1q2*q0q3q1q2/d0/d0+1.0f)*(4.0f*q0q3q1q2*q0q3q1q2/d0/d0+1.0f);
NaotoMorita 73:5770a0d470c0 1043
NaotoMorita 73:5770a0d470c0 1044
NaotoMorita 73:5770a0d470c0 1045
NaotoMorita 73:5770a0d470c0 1046 Matrix Hh(2,4);
NaotoMorita 73:5770a0d470c0 1047 Hh(1,1) = -(8.0f*q3*q0q3q1q2/d0/d0-16.0f*q0*q0q3q1q2*q0q3q1q2/d0/d0/d0)/h1lower;
NaotoMorita 73:5770a0d470c0 1048 Hh(1,2) = -(8.0f*q2*q0q3q1q2/d0/d0-16.0f*q1*q0q3q1q2*q0q3q1q2/d0/d0/d0)/h1lower;
NaotoMorita 73:5770a0d470c0 1049 Hh(1,3) = -(8.0f*q1*q0q3q1q2/d0/d0-16.0f*q2*q0q3q1q2*q0q3q1q2/d0/d0/d0)/h1lower;
NaotoMorita 73:5770a0d470c0 1050 Hh(1,4) = -(8.0f*q0*q0q3q1q2/d0/d0-16.0f*q3*q0q3q1q2*q0q3q1q2/d0/d0/d0)/h1lower;
NaotoMorita 73:5770a0d470c0 1051
NaotoMorita 73:5770a0d470c0 1052 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 1053 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 1054 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 1055 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 1056 */