HAPSRG / solaESKF_Eigen

Eigen

Dependencies:   Eigen

Dependents:   optWingforHAPS_Eigen hexaTest_Eigen

solaESKF.cpp@88:06d5b2cc5e20, 16 months ago (annotated)

Committer:
NaotoMorita
Date:
Wed Jan 11 04:11:17 2023 +0000
Revision:
88:06d5b2cc5e20
Parent:
86:cc2c0025d820 
fuse center cpp

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 88:06d5b2cc5e20 559 void solaESKF::fuseCenter2Nominal(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 86:cc2c0025d820 655 Vector3f solaESKF::vector2Body(Vector3f veci)
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 86:cc2c0025d820 661 return tdcm*veci ;
NaotoMorita 82:662b810155e9 662 }
NaotoMorita 82:662b810155e9 663
NaotoMorita 86:cc2c0025d820 664 Vector3f solaESKF::vector2NED(Vector3f vecb)
NaotoMorita 86:cc2c0025d820 665 {
NaotoMorita 86:cc2c0025d820 666 Matrix3f dcm;
NaotoMorita 86:cc2c0025d820 667 computeDcm(dcm, qhat);
NaotoMorita 86:cc2c0025d820 668
NaotoMorita 86:cc2c0025d820 669 return dcm*vecb;
NaotoMorita 86:cc2c0025d820 670 }
NaotoMorita 86:cc2c0025d820 671
NaotoMorita 86:cc2c0025d820 672
NaotoMorita 47:2467de40951f 673
NaotoMorita 44:7d82e63b6a86 674 void solaESKF::setGravity(float gx,float gy,float gz)
NaotoMorita 23:1509648c2318 675 {
cocorlow 78:e36a7b844fb5 676 gravity(0) = gx;
cocorlow 78:e36a7b844fb5 677 gravity(1) = gy;
cocorlow 78:e36a7b844fb5 678 gravity(2) = gz;
NaotoMorita 23:1509648c2318 679 }
NaotoMorita 62:5519d34eb6e8 680
cocorlow 78:e36a7b844fb5 681 Vector3f solaESKF::getPihat()
NaotoMorita 47:2467de40951f 682 {
NaotoMorita 47:2467de40951f 683 return pihat;
NaotoMorita 47:2467de40951f 684 }
cocorlow 78:e36a7b844fb5 685 Vector3f solaESKF::getVihat()
NaotoMorita 47:2467de40951f 686 {
NaotoMorita 47:2467de40951f 687 return vihat;
NaotoMorita 47:2467de40951f 688 }
cocorlow 78:e36a7b844fb5 689 Vector4f solaESKF::getQhat()
NaotoMorita 47:2467de40951f 690 {
NaotoMorita 47:2467de40951f 691 return qhat;
NaotoMorita 47:2467de40951f 692 }
cocorlow 78:e36a7b844fb5 693 Vector3f solaESKF::getAccBias()
NaotoMorita 47:2467de40951f 694 {
NaotoMorita 47:2467de40951f 695 return accBias;
NaotoMorita 47:2467de40951f 696 }
cocorlow 78:e36a7b844fb5 697 Vector3f solaESKF::getGyroBias()
NaotoMorita 47:2467de40951f 698 {
NaotoMorita 47:2467de40951f 699 return gyroBias;
NaotoMorita 47:2467de40951f 700 }
cocorlow 78:e36a7b844fb5 701 Vector3f solaESKF::getGravity()
NaotoMorita 47:2467de40951f 702 {
NaotoMorita 47:2467de40951f 703 return gravity;
NaotoMorita 47:2467de40951f 704 }
NaotoMorita 74:f5fe7fecbd3c 705
cocorlow 78:e36a7b844fb5 706 VectorXf solaESKF::getErrState()
NaotoMorita 47:2467de40951f 707 {
NaotoMorita 47:2467de40951f 708 return errState;
NaotoMorita 47:2467de40951f 709 }
NaotoMorita 23:1509648c2318 710
NaotoMorita 80:b241c058df83 711 VectorXf solaESKF::getState()
NaotoMorita 47:2467de40951f 712 {
NaotoMorita 80:b241c058df83 713 VectorXf state = VectorXf::Zero(nState+1);
NaotoMorita 80:b241c058df83 714 for (int i = 0; i < 3; i++){
NaotoMorita 80:b241c058df83 715 state(i) = pihat(i);
NaotoMorita 80:b241c058df83 716 state(i+3) = vihat(i);
NaotoMorita 80:b241c058df83 717 }
NaotoMorita 80:b241c058df83 718 for (int i = 0; i < 4; i++){
NaotoMorita 80:b241c058df83 719 state(i+6) = qhat(i);
NaotoMorita 80:b241c058df83 720 }
NaotoMorita 80:b241c058df83 721 for (int i = 0; i < 3; i++){
NaotoMorita 80:b241c058df83 722 state(i+10) = accBias(i);
NaotoMorita 80:b241c058df83 723 state(i+13) = gyroBias(i);
NaotoMorita 80:b241c058df83 724 state(i+16) = gravity(i);
NaotoMorita 80:b241c058df83 725 }
NaotoMorita 80:b241c058df83 726 return state;
NaotoMorita 47:2467de40951f 727 }
NaotoMorita 80:b241c058df83 728
NaotoMorita 80:b241c058df83 729 VectorXf solaESKF::getVariance()
NaotoMorita 47:2467de40951f 730 {
NaotoMorita 80:b241c058df83 731 VectorXf variance = VectorXf::Zero(nState);
NaotoMorita 80:b241c058df83 732 for (int i = 0; i < nState; i++){
NaotoMorita 80:b241c058df83 733 variance(i) = Phat(i,i);
NaotoMorita 80:b241c058df83 734 }
NaotoMorita 80:b241c058df83 735 return variance;
NaotoMorita 80:b241c058df83 736 }
NaotoMorita 80:b241c058df83 737
NaotoMorita 80:b241c058df83 738 void solaESKF::setPhatPosition(float valNE,float valD)
NaotoMorita 80:b241c058df83 739 {
NaotoMorita 80:b241c058df83 740 setBlockDiag(Phat, valNE, 0,2);
NaotoMorita 80:b241c058df83 741 Phat(2,2) = valD;
NaotoMorita 80:b241c058df83 742 }
NaotoMorita 80:b241c058df83 743 void solaESKF::setPhatVelocity(float valNE,float valD)
NaotoMorita 80:b241c058df83 744 {
NaotoMorita 80:b241c058df83 745 setBlockDiag(Phat, valNE, 3,5);
NaotoMorita 80:b241c058df83 746 Phat(5,5) = valD;
NaotoMorita 47:2467de40951f 747 }
NaotoMorita 47:2467de40951f 748 void solaESKF::setPhatAngleError(float val)
NaotoMorita 47:2467de40951f 749 {
cocorlow 78:e36a7b844fb5 750 setBlockDiag(Phat, val, 6,8);
NaotoMorita 47:2467de40951f 751 }
NaotoMorita 47:2467de40951f 752 void solaESKF::setPhatAccBias(float val)
NaotoMorita 47:2467de40951f 753 {
cocorlow 78:e36a7b844fb5 754 setBlockDiag(Phat, val, 9,11);
NaotoMorita 47:2467de40951f 755 }
NaotoMorita 47:2467de40951f 756 void solaESKF::setPhatGyroBias(float val)
NaotoMorita 47:2467de40951f 757 {
cocorlow 78:e36a7b844fb5 758 setBlockDiag(Phat, val, 12,14);
NaotoMorita 47:2467de40951f 759 }
NaotoMorita 47:2467de40951f 760 void solaESKF::setPhatGravity(float val)
NaotoMorita 47:2467de40951f 761 {
cocorlow 78:e36a7b844fb5 762 setBlockDiag(Phat, val, 15,17);
NaotoMorita 62:5519d34eb6e8 763 }
NaotoMorita 47:2467de40951f 764
NaotoMorita 47:2467de40951f 765
NaotoMorita 80:b241c058df83 766 void solaESKF::setQVelocity(float valNE,float valD)
NaotoMorita 47:2467de40951f 767 {
NaotoMorita 80:b241c058df83 768 setBlockDiag(Q, valNE, 3, 5);
NaotoMorita 80:b241c058df83 769 Q(5,5) = valD;
NaotoMorita 47:2467de40951f 770 }
NaotoMorita 47:2467de40951f 771 void solaESKF::setQAngleError(float val)
NaotoMorita 47:2467de40951f 772 {
cocorlow 78:e36a7b844fb5 773 setBlockDiag(Q, val, 6, 8);
NaotoMorita 47:2467de40951f 774 }
NaotoMorita 47:2467de40951f 775 void solaESKF::setQAccBias(float val)
NaotoMorita 47:2467de40951f 776 {
cocorlow 78:e36a7b844fb5 777 setBlockDiag(Q, val, 9, 11);
NaotoMorita 47:2467de40951f 778 }
NaotoMorita 47:2467de40951f 779 void solaESKF::setQGyroBias(float val)
NaotoMorita 47:2467de40951f 780 {
cocorlow 78:e36a7b844fb5 781 setBlockDiag(Q, val, 12, 14);
NaotoMorita 47:2467de40951f 782 }
NaotoMorita 19:3fae66745363 783
NaotoMorita 80:b241c058df83 784 void solaESKF::setDiag(Matrix3f& mat, float val){
NaotoMorita 80:b241c058df83 785 for (int i = 0; i < mat.cols(); i++){
NaotoMorita 80:b241c058df83 786 for (int j = 0; j < mat.rows(); j++){
NaotoMorita 80:b241c058df83 787 mat(i,j) = 0.0f;
NaotoMorita 80:b241c058df83 788 }
NaotoMorita 80:b241c058df83 789 }
cocorlow 78:e36a7b844fb5 790 for (int i = 0; i < mat.cols(); i++){
NaotoMorita 44:7d82e63b6a86 791 mat(i,i) = val;
NaotoMorita 22:7d84b8bc20b4 792 }
NaotoMorita 22:7d84b8bc20b4 793 }
NaotoMorita 22:7d84b8bc20b4 794
NaotoMorita 80:b241c058df83 795 void solaESKF::setDiag(MatrixXf& mat, float val){
NaotoMorita 80:b241c058df83 796 for (int i = 0; i < mat.cols(); i++){
NaotoMorita 80:b241c058df83 797 for (int j = 0; j < mat.rows(); j++){
NaotoMorita 80:b241c058df83 798 mat(i,j) = 0.0f;
NaotoMorita 80:b241c058df83 799 }
NaotoMorita 80:b241c058df83 800 }
NaotoMorita 80:b241c058df83 801 for (int i = 0; i < mat.cols(); i++)
NaotoMorita 80:b241c058df83 802 {
NaotoMorita 80:b241c058df83 803 mat(i, i) = val;
NaotoMorita 80:b241c058df83 804 }
NaotoMorita 80:b241c058df83 805 }
NaotoMorita 80:b241c058df83 806
cocorlow 78:e36a7b844fb5 807 void solaESKF::setBlockDiag(MatrixXf& mat, float val, int startIndex, int endIndex){
NaotoMorita 80:b241c058df83 808
NaotoMorita 44:7d82e63b6a86 809 for (int i = startIndex; i < endIndex+1; i++){
NaotoMorita 44:7d82e63b6a86 810 mat(i,i) = val;
NaotoMorita 22:7d84b8bc20b4 811 }
NaotoMorita 22:7d84b8bc20b4 812 }
osaka 51:a5af3b280d23 813
NaotoMorita 81:230a3d2b0493 814 void solaESKF::setPihat(float pi_x, float pi_y, float pi_z)
osaka 51:a5af3b280d23 815 {
cocorlow 78:e36a7b844fb5 816 pihat(0) = pi_x;
cocorlow 78:e36a7b844fb5 817 pihat(1) = pi_y;
NaotoMorita 82:662b810155e9 818 pihat(2) = pi_z;
NaotoMorita 56:c10f1168bd4a 819 }
NaotoMorita 73:5770a0d470c0 820
cocorlow 78:e36a7b844fb5 821 Matrix3f solaESKF::Matrixcross(Vector3f v)
cocorlow 78:e36a7b844fb5 822 {
NaotoMorita 83:8f6ae61d47ac 823 Matrix3f m = Matrix3f::Zero();
cocorlow 78:e36a7b844fb5 824 m << 0.0f, -v(2), v(1),
cocorlow 78:e36a7b844fb5 825 v(2), 0.0f, -v(0),
cocorlow 78:e36a7b844fb5 826 -v(1), v(0), 0.0f;
cocorlow 78:e36a7b844fb5 827 return m;
cocorlow 78:e36a7b844fb5 828 }
NaotoMorita 56:c10f1168bd4a 829 /*
NaotoMorita 56:c10f1168bd4a 830 void solaESKF::updateAccConstraints(Matrix acc,float palt,Matrix R)
NaotoMorita 56:c10f1168bd4a 831 {
NaotoMorita 56:c10f1168bd4a 832 Matrix accm = acc - accBias;
NaotoMorita 56:c10f1168bd4a 833 Matrix tdcm(3,3);
NaotoMorita 56:c10f1168bd4a 834 computeDcm(tdcm, qhat);
NaotoMorita 56:c10f1168bd4a 835 tdcm = MatrixMath::Transpose(tdcm);
NaotoMorita 56:c10f1168bd4a 836 Matrix tdcm_g = tdcm*gravity;
NaotoMorita 56:c10f1168bd4a 837 Matrix a2v = MatrixMath::Matrixcross(tdcm_g(1,1),tdcm_g(2,1),tdcm_g(3,1));
NaotoMorita 56:c10f1168bd4a 838
NaotoMorita 56:c10f1168bd4a 839 Matrix H(4,nState);
NaotoMorita 56:c10f1168bd4a 840 for (int i = 1; i < 4; i++){
NaotoMorita 56:c10f1168bd4a 841 for (int j = 1; j < 4; j++){
NaotoMorita 56:c10f1168bd4a 842 H(i,j+6) = a2v(i,j);
NaotoMorita 56:c10f1168bd4a 843 H(i,j+15) = tdcm(i,j);
NaotoMorita 56:c10f1168bd4a 844 }
NaotoMorita 56:c10f1168bd4a 845 }
NaotoMorita 56:c10f1168bd4a 846 H(1,10) = -1.0f;
NaotoMorita 56:c10f1168bd4a 847 H(2,11) = -1.0f;
NaotoMorita 56:c10f1168bd4a 848 H(3,12) = -1.0f;
NaotoMorita 56:c10f1168bd4a 849 H(4,3) = 1.0f;
NaotoMorita 56:c10f1168bd4a 850
NaotoMorita 56:c10f1168bd4a 851 //Matrix A = H*Phat*MatrixMath::Transpose(H)+R;
NaotoMorita 56:c10f1168bd4a 852 //Matrix K = (Phat*MatrixMath::Transpose(H))*(MatrixMath::Inv(MatrixMath::Transpose(A)*A+10.0f*MatrixMath::Eye(3))*MatrixMath::Transpose(A));
NaotoMorita 56:c10f1168bd4a 853 Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
NaotoMorita 56:c10f1168bd4a 854 Matrix zacc = -accm-tdcm*gravity;
NaotoMorita 56:c10f1168bd4a 855 Matrix z(4,1);
NaotoMorita 56:c10f1168bd4a 856 z << zacc(1,1) << zacc(2,1) << zacc(3,1) << palt - pihat(3,1);
NaotoMorita 56:c10f1168bd4a 857 errState = K * z;
NaotoMorita 56:c10f1168bd4a 858 Phat = (MatrixMath::Eye(nState)-K*H)*Phat;
NaotoMorita 56:c10f1168bd4a 859 //Phat = Phat-K*(H*Phat*MatrixMath::Transpose(H)+R)*MatrixMath::Transpose(K);
NaotoMorita 56:c10f1168bd4a 860 //Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*R*MatrixMath::Transpose(K);
NaotoMorita 56:c10f1168bd4a 861 fuseErr2Nominal();
NaotoMorita 56:c10f1168bd4a 862 }
NaotoMorita 56:c10f1168bd4a 863
NaotoMorita 56:c10f1168bd4a 864 void solaESKF::updateGyroConstraints(Matrix gyro,Matrix R)
NaotoMorita 56:c10f1168bd4a 865 {
NaotoMorita 56:c10f1168bd4a 866 Matrix gyrom = gyro - gyroBias;
NaotoMorita 56:c10f1168bd4a 867 Matrix dcm(3,3);
NaotoMorita 56:c10f1168bd4a 868 computeDcm(dcm, qhat);
NaotoMorita 56:c10f1168bd4a 869 Matrix a2v = dcm*MatrixMath::Matrixcross(gyrom(1,1),gyrom(2,1),gyrom(3,1));
NaotoMorita 56:c10f1168bd4a 870
NaotoMorita 56:c10f1168bd4a 871 Matrix H(2,nState);
NaotoMorita 56:c10f1168bd4a 872 for (int i = 1; i < 3; i++){
NaotoMorita 56:c10f1168bd4a 873 for (int j = 1; j < 4; j++){
NaotoMorita 56:c10f1168bd4a 874 H(i,j+6) = a2v(i,j);
NaotoMorita 56:c10f1168bd4a 875 H(i,j+12) = -dcm(i,j);
NaotoMorita 56:c10f1168bd4a 876 }
NaotoMorita 56:c10f1168bd4a 877 }
NaotoMorita 56:c10f1168bd4a 878 //Matrix A = H*Phat*MatrixMath::Transpose(H)+R;
NaotoMorita 56:c10f1168bd4a 879 //Matrix K = (Phat*MatrixMath::Transpose(H))*(MatrixMath::Inv(MatrixMath::Transpose(A)*A+10.0f*MatrixMath::Eye(2))*MatrixMath::Transpose(A));
NaotoMorita 56:c10f1168bd4a 880 Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
NaotoMorita 56:c10f1168bd4a 881
NaotoMorita 56:c10f1168bd4a 882 Matrix z3 = -dcm*gyrom;
NaotoMorita 56:c10f1168bd4a 883 Matrix z(2,1);
NaotoMorita 56:c10f1168bd4a 884 z << z3(1,1) << z3(2,1);
NaotoMorita 56:c10f1168bd4a 885 errState = K * z;
NaotoMorita 56:c10f1168bd4a 886 //Phat = (MatrixMath::Eye(nState)-K*H)*Phat;
NaotoMorita 56:c10f1168bd4a 887 //Phat = Phat-K*(H*Phat*MatrixMath::Transpose(H)+R)*MatrixMath::Transpose(K);
NaotoMorita 56:c10f1168bd4a 888 Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*R*MatrixMath::Transpose(K);
NaotoMorita 56:c10f1168bd4a 889 fuseErr2Nominal();
NaotoMorita 56:c10f1168bd4a 890 }
NaotoMorita 56:c10f1168bd4a 891 void solaESKF::updateMag(Matrix mag, Matrix R)
NaotoMorita 56:c10f1168bd4a 892 {
NaotoMorita 56:c10f1168bd4a 893 Matrix dcm(3,3);
NaotoMorita 56:c10f1168bd4a 894 computeDcm(dcm, qhat);
NaotoMorita 56:c10f1168bd4a 895 Matrix a2v = -dcm*MatrixMath::Matrixcross(mag(1,1),mag(2,1),mag(3,1));
NaotoMorita 56:c10f1168bd4a 896
NaotoMorita 56:c10f1168bd4a 897 Matrix H(2,nState);
NaotoMorita 56:c10f1168bd4a 898 for (int i = 1; i < 3; i++){
NaotoMorita 56:c10f1168bd4a 899 for (int j = 1; j < 4; j++){
NaotoMorita 56:c10f1168bd4a 900 H(i,j+6) = a2v(i,j);
NaotoMorita 56:c10f1168bd4a 901 }
NaotoMorita 56:c10f1168bd4a 902 }
NaotoMorita 56:c10f1168bd4a 903 H(1,19) = 1.0f;
NaotoMorita 56:c10f1168bd4a 904 //H(3,20) = 1.0f;
NaotoMorita 56:c10f1168bd4a 905
NaotoMorita 56:c10f1168bd4a 906 //Matrix A = H*Phat*MatrixMath::Transpose(H)+R;
NaotoMorita 56:c10f1168bd4a 907 //Matrix K = (Phat*MatrixMath::Transpose(H))*(MatrixMath::Inv(MatrixMath::Transpose(A)*A+10.0f*MatrixMath::Eye(3))*MatrixMath::Transpose(A));
NaotoMorita 56:c10f1168bd4a 908 Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
NaotoMorita 56:c10f1168bd4a 909 Matrix zmag = -dcm*mag-magField;
NaotoMorita 56:c10f1168bd4a 910 Matrix z(2,1);
NaotoMorita 56:c10f1168bd4a 911 z << zmag(1,1) << zmag(2,1);
NaotoMorita 56:c10f1168bd4a 912 errState = K * z;
NaotoMorita 56:c10f1168bd4a 913 Phat = (MatrixMath::Eye(nState)-K*H)*Phat;
NaotoMorita 56:c10f1168bd4a 914 //Phat = Phat-K*(H*Phat*MatrixMath::Transpose(H)+R)*MatrixMath::Transpose(K);
NaotoMorita 56:c10f1168bd4a 915 //Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*R*MatrixMath::Transpose(K);
NaotoMorita 56:c10f1168bd4a 916 fuseErr2Nominal();
NaotoMorita 56:c10f1168bd4a 917 }
NaotoMorita 59:03fe5e16a33c 918 void solaESKF::updateMag(Matrix mag,float palt, Matrix R)
NaotoMorita 59:03fe5e16a33c 919 {
NaotoMorita 59:03fe5e16a33c 920 Matrix dcm(3,3);
NaotoMorita 59:03fe5e16a33c 921 computeDcm(dcm, qhat);
NaotoMorita 59:03fe5e16a33c 922 Matrix a2v = -dcm*MatrixMath::Matrixcross(mag(1,1),mag(2,1),mag(3,1));
NaotoMorita 59:03fe5e16a33c 923
NaotoMorita 59:03fe5e16a33c 924 Matrix H(3,nState);
NaotoMorita 59:03fe5e16a33c 925 for (int i = 1; i < 3; i++){
NaotoMorita 59:03fe5e16a33c 926 for (int j = 1; j < 4; j++){
NaotoMorita 59:03fe5e16a33c 927 H(i,j+6) = a2v(i,j);
NaotoMorita 59:03fe5e16a33c 928 }
NaotoMorita 59:03fe5e16a33c 929 }
NaotoMorita 59:03fe5e16a33c 930 H(1,19) = 1.0f;
NaotoMorita 59:03fe5e16a33c 931 H(3,3) = 1.0f;
NaotoMorita 59:03fe5e16a33c 932 //Matrix A = H*Phat*MatrixMath::Transpose(H)+R;
NaotoMorita 59:03fe5e16a33c 933 //Matrix K = (Phat*MatrixMath::Transpose(H))*(MatrixMath::Inv(MatrixMath::Transpose(A)*A+10.0f*MatrixMath::Eye(3))*MatrixMath::Transpose(A));
NaotoMorita 59:03fe5e16a33c 934 Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
NaotoMorita 59:03fe5e16a33c 935 Matrix zmag = -dcm*mag-magField;
NaotoMorita 59:03fe5e16a33c 936 Matrix z(3,1);
NaotoMorita 59:03fe5e16a33c 937 z << zmag(1,1) << zmag(2,1) << palt - pihat(3,1);
NaotoMorita 59:03fe5e16a33c 938 errState = K * z;
NaotoMorita 59:03fe5e16a33c 939 Phat = (MatrixMath::Eye(nState)-K*H)*Phat;
NaotoMorita 59:03fe5e16a33c 940 //Phat = Phat-K*(H*Phat*MatrixMath::Transpose(H)+R)*MatrixMath::Transpose(K);
NaotoMorita 59:03fe5e16a33c 941 //Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*R*MatrixMath::Transpose(K);
NaotoMorita 59:03fe5e16a33c 942
NaotoMorita 59:03fe5e16a33c 943 fuseErr2Nominal();
NaotoMorita 59:03fe5e16a33c 944 }
NaotoMorita 59:03fe5e16a33c 945 void solaESKF::updateGPSVelocity(Matrix velgps,Matrix mag,Matrix R)
NaotoMorita 59:03fe5e16a33c 946 {
NaotoMorita 59:03fe5e16a33c 947
NaotoMorita 59:03fe5e16a33c 948 Matrix dcm(3,3);
NaotoMorita 59:03fe5e16a33c 949 computeDcm(dcm, qhat);
NaotoMorita 59:03fe5e16a33c 950 Matrix a2v = -dcm*MatrixMath::Matrixcross(mag(1,1),mag(2,1),mag(3,1));
NaotoMorita 59:03fe5e16a33c 951
NaotoMorita 59:03fe5e16a33c 952
NaotoMorita 59:03fe5e16a33c 953 Matrix H(3,nState);
NaotoMorita 59:03fe5e16a33c 954 H(1,4) = 1.0f;
NaotoMorita 59:03fe5e16a33c 955 H(2,5) = 1.0f;
NaotoMorita 59:03fe5e16a33c 956
NaotoMorita 59:03fe5e16a33c 957 for (int j = 1; j < 4; j++){
NaotoMorita 59:03fe5e16a33c 958 H(3,j+6) = a2v(2,j);
NaotoMorita 59:03fe5e16a33c 959 }
NaotoMorita 59:03fe5e16a33c 960 //H(3,19) = 1.0f;
NaotoMorita 59:03fe5e16a33c 961
NaotoMorita 59:03fe5e16a33c 962 Matrix zmag = -dcm*mag-magField;
NaotoMorita 59:03fe5e16a33c 963
NaotoMorita 59:03fe5e16a33c 964 //Matrix A = H*Phat*MatrixMath::Transpose(H)+R;
NaotoMorita 59:03fe5e16a33c 965 //Matrix K = (Phat*MatrixMath::Transpose(H))*(MatrixMath::Inv(MatrixMath::Transpose(A)*A+10.0f*MatrixMath::Eye(3))*MatrixMath::Transpose(A));
NaotoMorita 59:03fe5e16a33c 966 Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
NaotoMorita 59:03fe5e16a33c 967 Matrix z(3,1);
NaotoMorita 59:03fe5e16a33c 968 z << velgps(1,1) - vihat(1,1) << velgps(2,1)-vihat(2,1) << zmag(2,1);
NaotoMorita 59:03fe5e16a33c 969 errState = K * z;
NaotoMorita 59:03fe5e16a33c 970 Phat = (MatrixMath::Eye(nState)-K*H)*Phat;
NaotoMorita 59:03fe5e16a33c 971 //Phat = Phat-K*(H*Phat*MatrixMath::Transpose(H)+R)*MatrixMath::Transpose(K);
NaotoMorita 59:03fe5e16a33c 972 //Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*R*MatrixMath::Transpose(K);
NaotoMorita 59:03fe5e16a33c 973 fuseErr2Nominal();
NaotoMorita 59:03fe5e16a33c 974 }
NaotoMorita 56:c10f1168bd4a 975
NaotoMorita 59:03fe5e16a33c 976 void solaESKF::updateGPSPosition(Matrix posgps,float palt,Matrix R)
NaotoMorita 59:03fe5e16a33c 977 {
NaotoMorita 59:03fe5e16a33c 978 Matrix H(3,nState);
NaotoMorita 59:03fe5e16a33c 979 H(1,1) = 1.0f;
NaotoMorita 59:03fe5e16a33c 980 H(2,2) = 1.0f;
NaotoMorita 59:03fe5e16a33c 981 H(3,3) = 1.0f;
NaotoMorita 59:03fe5e16a33c 982
NaotoMorita 59:03fe5e16a33c 983 //Matrix A = H*Phat*MatrixMath::Transpose(H)+R;
NaotoMorita 59:03fe5e16a33c 984 //Matrix K = (Phat*MatrixMath::Transpose(H))*(MatrixMath::Inv(MatrixMath::Transpose(A)*A+1000.0f*MatrixMath::Eye(3))*MatrixMath::Transpose(A));
NaotoMorita 59:03fe5e16a33c 985 Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
NaotoMorita 59:03fe5e16a33c 986 Matrix z(3,1);
NaotoMorita 59:03fe5e16a33c 987 z << posgps(1,1) - pihat(1,1) << posgps(2,1)-pihat(2,1) << palt - pihat(3,1);
NaotoMorita 59:03fe5e16a33c 988 errState = K * z;
NaotoMorita 59:03fe5e16a33c 989 Phat = (MatrixMath::Eye(nState)-K*H)*Phat;
NaotoMorita 59:03fe5e16a33c 990 //Phat = Phat-K*(H*Phat*MatrixMath::Transpose(H)+R)*MatrixMath::Transpose(K);
NaotoMorita 59:03fe5e16a33c 991 //Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*R*MatrixMath::Transpose(K);
NaotoMorita 59:03fe5e16a33c 992 fuseErr2Nominal();
NaotoMorita 59:03fe5e16a33c 993 }
NaotoMorita 61:5e5c4fe12440 994
NaotoMorita 61:5e5c4fe12440 995 void solaESKF::updateImuConstraints(Matrix acc,Matrix mag,Matrix R)
NaotoMorita 61:5e5c4fe12440 996 {
NaotoMorita 61:5e5c4fe12440 997 Matrix accm = acc - accBias;
NaotoMorita 61:5e5c4fe12440 998 Matrix magm = mag - magBias;
NaotoMorita 61:5e5c4fe12440 999 Matrix dcm(3,3);
NaotoMorita 61:5e5c4fe12440 1000 computeDcm(dcm, qhat);
NaotoMorita 61:5e5c4fe12440 1001 Matrix tdcm = MatrixMath::Transpose(dcm);
NaotoMorita 61:5e5c4fe12440 1002 Matrix tdcm_g = tdcm*gravity;
NaotoMorita 61:5e5c4fe12440 1003 Matrix rotgrav = MatrixMath::Matrixcross(tdcm_g(1,1),tdcm_g(2,1),tdcm_g(3,1));
NaotoMorita 61:5e5c4fe12440 1004 Matrix rotmag = -dcm*MatrixMath::Matrixcross(magm(1,1),magm(2,1),magm(3,1));
NaotoMorita 61:5e5c4fe12440 1005
NaotoMorita 61:5e5c4fe12440 1006 Matrix H(5,nState);
NaotoMorita 61:5e5c4fe12440 1007 for (int i = 1; i < 4; i++){
NaotoMorita 61:5e5c4fe12440 1008 for (int j = 1; j < 4; j++){
NaotoMorita 61:5e5c4fe12440 1009 H(i,j+6) = rotgrav(i,j);
NaotoMorita 61:5e5c4fe12440 1010 }
NaotoMorita 61:5e5c4fe12440 1011 H(i,16) = tdcm(i,3);
NaotoMorita 61:5e5c4fe12440 1012 }
NaotoMorita 61:5e5c4fe12440 1013
NaotoMorita 61:5e5c4fe12440 1014 H(1,10) = -1.0f;
NaotoMorita 61:5e5c4fe12440 1015 H(2,11) = -1.0f;
NaotoMorita 61:5e5c4fe12440 1016 H(3,12) = -1.0f;
NaotoMorita 61:5e5c4fe12440 1017
NaotoMorita 61:5e5c4fe12440 1018 Matrix magned = dcm*magm;
NaotoMorita 61:5e5c4fe12440 1019 float hx = sqrt(magned(1,1)*magned(1,1)+magned(2,1)*magned(2,1));
NaotoMorita 61:5e5c4fe12440 1020
NaotoMorita 61:5e5c4fe12440 1021 for(int j = 1; j < 4; j++){
NaotoMorita 61:5e5c4fe12440 1022 H(4,j+6) = rotmag(1,j)-(rotmag(1,j)+rotmag(2,j))/hx;
NaotoMorita 61:5e5c4fe12440 1023 H(4,j+16) = -dcm(1,j)+(dcm(1,j)+dcm(2,j))/hx;
NaotoMorita 61:5e5c4fe12440 1024 H(5,j+6) = rotmag(2,j);
NaotoMorita 61:5e5c4fe12440 1025 H(5,j+16) = -dcm(2,j);
NaotoMorita 61:5e5c4fe12440 1026 }
NaotoMorita 61:5e5c4fe12440 1027
NaotoMorita 61:5e5c4fe12440 1028 Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
NaotoMorita 61:5e5c4fe12440 1029 Matrix zacc = -accm-tdcm*gravity;
NaotoMorita 61:5e5c4fe12440 1030 Matrix zmag = dcm*magm;
NaotoMorita 61:5e5c4fe12440 1031 Matrix z(5,1);
NaotoMorita 61:5e5c4fe12440 1032 z << zacc(1,1) << zacc(2,1) << zacc(3,1) << -(zmag(1,1) - hx) << -zmag(2,1);
NaotoMorita 61:5e5c4fe12440 1033 twelite.printf("%f %f\r\n",hx,(zmag(1,1) - hx));
NaotoMorita 61:5e5c4fe12440 1034 errState = K * z;
NaotoMorita 61:5e5c4fe12440 1035 Phat = (MatrixMath::Eye(nState)-K*H)*Phat;
NaotoMorita 61:5e5c4fe12440 1036
NaotoMorita 61:5e5c4fe12440 1037 fuseErr2Nominal();
NaotoMorita 61:5e5c4fe12440 1038 }
NaotoMorita 73:5770a0d470c0 1039 float q0 = qhat(1,1);
NaotoMorita 73:5770a0d470c0 1040 float q1 = qhat(2,1);
NaotoMorita 73:5770a0d470c0 1041 float q2 = qhat(3,1);
NaotoMorita 73:5770a0d470c0 1042 float q3 = qhat(4,1);
NaotoMorita 73:5770a0d470c0 1043
NaotoMorita 73:5770a0d470c0 1044 float d0 = (-q3*q3-q2*q2+q1*q1+q0*q0);
NaotoMorita 73:5770a0d470c0 1045 float q0q3q1q2 = (q0*q3+q1*q2);
NaotoMorita 73:5770a0d470c0 1046 float h1lower = 2.0f*(4.0f*q0q3q1q2*q0q3q1q2/d0/d0+1.0f)*sqrt(4.0f*q0q3q1q2*q0q3q1q2/d0/d0+1.0f);
NaotoMorita 73:5770a0d470c0 1047
NaotoMorita 73:5770a0d470c0 1048 float d1 = d0*sqrt(4.0f*q0q3q1q2*q0q3q1q2/d0/d0+1.0f);
NaotoMorita 73:5770a0d470c0 1049 float d2 = d0*d0*sqrt(4.0f*q0q3q1q2*q0q3q1q2/d0/d0+1.0f);
NaotoMorita 73:5770a0d470c0 1050 float d3 = d0*sqrt(4.0f*q0q3q1q2*q0q3q1q2/d0/d0+1.0f)*(4.0f*q0q3q1q2*q0q3q1q2/d0/d0+1.0f);
NaotoMorita 73:5770a0d470c0 1051
NaotoMorita 73:5770a0d470c0 1052
NaotoMorita 73:5770a0d470c0 1053
NaotoMorita 73:5770a0d470c0 1054 Matrix Hh(2,4);
NaotoMorita 73:5770a0d470c0 1055 Hh(1,1) = -(8.0f*q3*q0q3q1q2/d0/d0-16.0f*q0*q0q3q1q2*q0q3q1q2/d0/d0/d0)/h1lower;
NaotoMorita 73:5770a0d470c0 1056 Hh(1,2) = -(8.0f*q2*q0q3q1q2/d0/d0-16.0f*q1*q0q3q1q2*q0q3q1q2/d0/d0/d0)/h1lower;
NaotoMorita 73:5770a0d470c0 1057 Hh(1,3) = -(8.0f*q1*q0q3q1q2/d0/d0-16.0f*q2*q0q3q1q2*q0q3q1q2/d0/d0/d0)/h1lower;
NaotoMorita 73:5770a0d470c0 1058 Hh(1,4) = -(8.0f*q0*q0q3q1q2/d0/d0-16.0f*q3*q0q3q1q2*q0q3q1q2/d0/d0/d0)/h1lower;
NaotoMorita 73:5770a0d470c0 1059
NaotoMorita 73:5770a0d470c0 1060 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 1061 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 1062 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 1063 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 1064 */