HAPSRG / solaESKF_Eigen

Eigen

Dependencies:   Eigen

Dependents:   optWingforHAPS_Eigen hexaTest_Eigen

solaESKF.cpp@45:df4618814803, 2021-10-26 (annotated)

Committer:
NaotoMorita
Date:
Tue Oct 26 05:36:11 2021 +0000
Revision:
45:df4618814803
Parent:
44:7d82e63b6a86
Child:
46:15988dc41923
solaeskf compiled

Who changed what in which revision?

UserRevisionLine numberNew contents of line
NaotoMorita 44:7d82e63b6a86 1 #include "solaESKF.hpp"
NaotoMorita 19:3fae66745363 2 #include "Matrix.h"
NaotoMorita 19:3fae66745363 3 #include "MatrixMath.h"
NaotoMorita 19:3fae66745363 4 #include <cmath>
NaotoMorita 19:3fae66745363 5
NaotoMorita 19:3fae66745363 6
NaotoMorita 44:7d82e63b6a86 7 solaESKF::solaESKF()
NaotoMorita 19:3fae66745363 8 {
NaotoMorita 44:7d82e63b6a86 9 //nominal state
NaotoMorita 44:7d82e63b6a86 10 pihat(3,1);
NaotoMorita 44:7d82e63b6a86 11 vihat(3,1);
NaotoMorita 44:7d82e63b6a86 12 qhat(4,1);
NaotoMorita 44:7d82e63b6a86 13 accBias(3,1);
NaotoMorita 44:7d82e63b6a86 14 gyroBias(3,1);
NaotoMorita 44:7d82e63b6a86 15 gravity(3,1);
NaotoMorita 44:7d82e63b6a86 16
NaotoMorita 44:7d82e63b6a86 17
NaotoMorita 44:7d82e63b6a86 18 errState(18,1);
NaotoMorita 21:d6079def0473 19 nState = errState.getRows();
NaotoMorita 44:7d82e63b6a86 20 Phat(nState,nState);
NaotoMorita 44:7d82e63b6a86 21 Q(nState,nState);
NaotoMorita 19:3fae66745363 22
osaka 39:6834e05d8a64 23 setDiag(Phat,0.1f);
NaotoMorita 44:7d82e63b6a86 24 setDiag(Q,0.01f);
NaotoMorita 44:7d82e63b6a86 25 setBlockDiag(Q,0.0f,1,3);
NaotoMorita 44:7d82e63b6a86 26 setBlockDiag(Q,0.00001f,16,18);
NaotoMorita 19:3fae66745363 27
NaotoMorita 19:3fae66745363 28
NaotoMorita 19:3fae66745363 29 }
NaotoMorita 19:3fae66745363 30
NaotoMorita 44:7d82e63b6a86 31 void solaESKF::updateNominal(Matrix acc, Matrix gyro,float att_dt)
NaotoMorita 19:3fae66745363 32 {
NaotoMorita 44:7d82e63b6a86 33 Matrix gyrom = gyro - gyroBias;
NaotoMorita 44:7d82e63b6a86 34 Matrix accm = acc - accBias;
NaotoMorita 44:7d82e63b6a86 35
NaotoMorita 44:7d82e63b6a86 36 Matrix qint(4,1);
NaotoMorita 44:7d82e63b6a86 37 qint << 1.0f << gyrom(1,1)*att_dt << gyrom(2,1)*att_dt << gyrom(3,1)*att_dt;
NaotoMorita 44:7d82e63b6a86 38 qhat = quatmultiply(qhat,qint);
NaotoMorita 19:3fae66745363 39 float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qhat),qhat));
NaotoMorita 19:3fae66745363 40 qhat *= (1.0f/ qnorm);
NaotoMorita 23:1509648c2318 41
NaotoMorita 23:1509648c2318 42 Matrix dcm(3,3);
NaotoMorita 23:1509648c2318 43 computeDcm(dcm, qhat);
NaotoMorita 44:7d82e63b6a86 44 Matrix accned = dcm*acc+gravity;
NaotoMorita 44:7d82e63b6a86 45 vihat += accned*att_dt;
NaotoMorita 44:7d82e63b6a86 46
NaotoMorita 44:7d82e63b6a86 47 pihat += vihat*att_dt+0.5f*accned*att_dt*att_dt;
NaotoMorita 19:3fae66745363 48 }
NaotoMorita 19:3fae66745363 49
NaotoMorita 44:7d82e63b6a86 50 void solaESKF::updateErrState(Matrix acc,Matrix gyro,float att_dt)
NaotoMorita 23:1509648c2318 51 {
NaotoMorita 44:7d82e63b6a86 52 Matrix gyrom = gyro - gyroBias;
NaotoMorita 44:7d82e63b6a86 53 Matrix accm = acc - accBias;
NaotoMorita 23:1509648c2318 54
NaotoMorita 23:1509648c2318 55 Matrix dcm(3,3);
NaotoMorita 23:1509648c2318 56 computeDcm(dcm, qhat);
NaotoMorita 44:7d82e63b6a86 57
NaotoMorita 44:7d82e63b6a86 58 Matrix Fx(nState,nState);
NaotoMorita 44:7d82e63b6a86 59 //position
NaotoMorita 44:7d82e63b6a86 60 Fx(1,1) = 1.0f;
NaotoMorita 44:7d82e63b6a86 61 Fx(2,2) = 1.0f;
NaotoMorita 44:7d82e63b6a86 62 Fx(3,3) = 1.0f;
NaotoMorita 44:7d82e63b6a86 63 Fx(1,4) = 1.0f*att_dt;
NaotoMorita 44:7d82e63b6a86 64 Fx(2,5) = 1.0f*att_dt;
NaotoMorita 44:7d82e63b6a86 65 Fx(3,6) = 1.0f*att_dt;
NaotoMorita 44:7d82e63b6a86 66
NaotoMorita 44:7d82e63b6a86 67 //velocity
NaotoMorita 44:7d82e63b6a86 68 Fx(4,4) = 1.0f;
NaotoMorita 44:7d82e63b6a86 69 Fx(5,5) = 1.0f;
NaotoMorita 44:7d82e63b6a86 70 Fx(6,6) = 1.0f;
NaotoMorita 44:7d82e63b6a86 71 Matrix a2v = -dcm*MatrixMath::Matrixcross(acc(1,1),acc(2,1),acc(3,1))*att_dt;
NaotoMorita 23:1509648c2318 72 for (int i = 1; i < 4; i++){
NaotoMorita 44:7d82e63b6a86 73 for (int j = 1; j < 4; j++){
NaotoMorita 44:7d82e63b6a86 74 Fx(i+3,j+6) = a2v(i,j);
NaotoMorita 44:7d82e63b6a86 75 Fx(i+3,j+9) = -dcm(i,j)*att_dt;
NaotoMorita 23:1509648c2318 76 }
NaotoMorita 23:1509648c2318 77 }
NaotoMorita 44:7d82e63b6a86 78 Fx(4,16) = 1.0f*att_dt;
NaotoMorita 44:7d82e63b6a86 79 Fx(5,17) = 1.0f*att_dt;
NaotoMorita 44:7d82e63b6a86 80 Fx(6,18) = 1.0f*att_dt;
NaotoMorita 23:1509648c2318 81
NaotoMorita 44:7d82e63b6a86 82 //angulat error
NaotoMorita 44:7d82e63b6a86 83 Fx(7,8) = gyrom(3,1)*att_dt;
NaotoMorita 44:7d82e63b6a86 84 Fx(7,9) = -gyrom(2,1)*att_dt;
NaotoMorita 44:7d82e63b6a86 85 Fx(8,7) = -gyrom(3,1)*att_dt;
NaotoMorita 44:7d82e63b6a86 86 Fx(8,9) = gyrom(1,1)*att_dt;
NaotoMorita 44:7d82e63b6a86 87 Fx(9,7) = gyrom(2,1)*att_dt;
NaotoMorita 44:7d82e63b6a86 88 Fx(9,8) = -gyrom(1,1)*att_dt;
NaotoMorita 44:7d82e63b6a86 89 Fx(7,13) = -1.0f*att_dt;
NaotoMorita 44:7d82e63b6a86 90 Fx(8,14) = -1.0f*att_dt;
NaotoMorita 44:7d82e63b6a86 91 Fx(9,15) = -1.0f*att_dt;
NaotoMorita 25:07ac5c6cd61c 92
NaotoMorita 44:7d82e63b6a86 93 //acc bias
NaotoMorita 44:7d82e63b6a86 94 Fx(10,10) = 1.0f;
NaotoMorita 44:7d82e63b6a86 95 Fx(11,11) = 1.0f;
NaotoMorita 44:7d82e63b6a86 96 Fx(12,12) = 1.0f;
NaotoMorita 44:7d82e63b6a86 97
NaotoMorita 44:7d82e63b6a86 98 //gyro bias
NaotoMorita 44:7d82e63b6a86 99 Fx(13,13) = 1.0f;
NaotoMorita 44:7d82e63b6a86 100 Fx(14,14) = 1.0f;
NaotoMorita 44:7d82e63b6a86 101 Fx(15,15) = 1.0f;
NaotoMorita 44:7d82e63b6a86 102
NaotoMorita 44:7d82e63b6a86 103 //gravity bias
NaotoMorita 44:7d82e63b6a86 104 Fx(16,16) = 1.0f;
NaotoMorita 44:7d82e63b6a86 105 Fx(17,17) = 1.0f;
NaotoMorita 44:7d82e63b6a86 106 Fx(18,18) = 1.0f;
NaotoMorita 44:7d82e63b6a86 107
NaotoMorita 44:7d82e63b6a86 108 errState = Fx * errState;
NaotoMorita 44:7d82e63b6a86 109 Phat = Fx*Phat*MatrixMath::Transpose(Fx)+Q*att_dt*att_dt;
NaotoMorita 25:07ac5c6cd61c 110 }
NaotoMorita 25:07ac5c6cd61c 111
NaotoMorita 44:7d82e63b6a86 112 /*
NaotoMorita 44:7d82e63b6a86 113 void solaESKF::updateGPSVelocity(float vi_x, float vi_y, float sinkRate,Vector3 acc, Vector3 accref)
NaotoMorita 19:3fae66745363 114 {
osaka 40:119792aa6d3b 115
NaotoMorita 43:cbc2c2d65131 116 Matrix H(3,nState);
NaotoMorita 43:cbc2c2d65131 117 H(1,10) = 1.0f;
NaotoMorita 43:cbc2c2d65131 118 H(2,11) = 1.0f;
NaotoMorita 43:cbc2c2d65131 119 H(3,12) = 1.0f;
NaotoMorita 43:cbc2c2d65131 120
osaka 40:119792aa6d3b 121 Matrix R(3,3);
osaka 40:119792aa6d3b 122 R(1,1) = Rgps(1,1);
osaka 40:119792aa6d3b 123 R(2,2) = Rgps(2,2);
NaotoMorita 43:cbc2c2d65131 124 R(3,3) = Rgps(3,3);
osaka 40:119792aa6d3b 125
NaotoMorita 31:e655d4d8e4d6 126 Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
NaotoMorita 43:cbc2c2d65131 127 Matrix z(3,1);
NaotoMorita 43:cbc2c2d65131 128 z << vi_x - vihat(1,1) << vi_y-vihat(2,1) << sinkRate - vihat(3,1);
osaka 40:119792aa6d3b 129 //z << vi_x - vihat(1,1) << vi_y-vihat(2,1) << sinkRate - vihat(3,1);
NaotoMorita 26:73c3f58b9d70 130 Matrix corrVal = K * (z-H*errState);
NaotoMorita 26:73c3f58b9d70 131 errState = errState + corrVal;
NaotoMorita 31:e655d4d8e4d6 132 Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*(R)*MatrixMath::Transpose(K);
NaotoMorita 19:3fae66745363 133 }
NaotoMorita 44:7d82e63b6a86 134 */
NaotoMorita 44:7d82e63b6a86 135 Matrix solaESKF::computeAngles()
NaotoMorita 25:07ac5c6cd61c 136 {
NaotoMorita 25:07ac5c6cd61c 137
NaotoMorita 44:7d82e63b6a86 138 Matrix euler(3,1);
NaotoMorita 44:7d82e63b6a86 139 euler(1,1) = atan2f(qhat(1,1)*qhat(2,1) + qhat(3,1)*qhat(4,1), 0.5f - qhat(2,1)*qhat(2,1) - qhat(3,1)*qhat(3,1));
NaotoMorita 44:7d82e63b6a86 140 euler(2,1) = asinf(-2.0f * (qhat(2,1)*qhat(4,1) - qhat(1,1)*qhat(3,1)));
NaotoMorita 44:7d82e63b6a86 141 euler(3,1) = atan2f(qhat(2,1)*qhat(3,1) + qhat(1,1)*qhat(4,1), 0.5f - qhat(3,1)*qhat(3,1) - qhat(4,1)*qhat(4,1));
NaotoMorita 44:7d82e63b6a86 142 return euler;
NaotoMorita 19:3fae66745363 143 }
NaotoMorita 19:3fae66745363 144
NaotoMorita 21:d6079def0473 145
NaotoMorita 44:7d82e63b6a86 146 void solaESKF::fuseErr2Nominal()
NaotoMorita 19:3fae66745363 147 {
NaotoMorita 44:7d82e63b6a86 148 //position
NaotoMorita 44:7d82e63b6a86 149 pihat(1,1) += errState(1,1);
NaotoMorita 44:7d82e63b6a86 150 pihat(2,1) += errState(2,1);
NaotoMorita 44:7d82e63b6a86 151 pihat(3,1) += errState(3,1);
NaotoMorita 44:7d82e63b6a86 152
NaotoMorita 44:7d82e63b6a86 153 //velocity
NaotoMorita 44:7d82e63b6a86 154 vihat(1,1) += errState(4,1);
NaotoMorita 44:7d82e63b6a86 155 vihat(2,1) += errState(5,1);
NaotoMorita 44:7d82e63b6a86 156 vihat(3,1) += errState(6,1);
NaotoMorita 44:7d82e63b6a86 157
NaotoMorita 44:7d82e63b6a86 158 //angle error
NaotoMorita 19:3fae66745363 159 Matrix qerr(4,1);
NaotoMorita 44:7d82e63b6a86 160 qerr << 1.0f << 0.5f*errState(1,7) << 0.5f*errState(8,1) << 0.5f*errState(9,1);
NaotoMorita 19:3fae66745363 161 qhat = quatmultiply(qhat, qerr);
NaotoMorita 19:3fae66745363 162 float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qhat),qhat));
NaotoMorita 19:3fae66745363 163 qhat *= (1.0f/ qnorm);
NaotoMorita 23:1509648c2318 164
NaotoMorita 44:7d82e63b6a86 165 //acc bias
NaotoMorita 44:7d82e63b6a86 166 accBias(1,1) += errState(10,1);
NaotoMorita 44:7d82e63b6a86 167 accBias(2,1) += errState(11,1);
NaotoMorita 44:7d82e63b6a86 168 accBias(3,1) += errState(12,1);
NaotoMorita 44:7d82e63b6a86 169
NaotoMorita 44:7d82e63b6a86 170 //gyro bias
NaotoMorita 44:7d82e63b6a86 171 gyroBias(1,1) += errState(13,1);
NaotoMorita 44:7d82e63b6a86 172 gyroBias(2,1) += errState(14,1);
NaotoMorita 44:7d82e63b6a86 173 gyroBias(3,1) += errState(15,1);
NaotoMorita 44:7d82e63b6a86 174
NaotoMorita 44:7d82e63b6a86 175 //gravity bias
NaotoMorita 44:7d82e63b6a86 176 gravity(1,1) += errState(16,1);
NaotoMorita 44:7d82e63b6a86 177 gravity(2,1) += errState(17,1);
NaotoMorita 44:7d82e63b6a86 178 gravity(3,1) += errState(18,1);
NaotoMorita 44:7d82e63b6a86 179
NaotoMorita 19:3fae66745363 180 }
NaotoMorita 19:3fae66745363 181
NaotoMorita 44:7d82e63b6a86 182 Matrix solaESKF::quatmultiply(Matrix p, Matrix q)
NaotoMorita 19:3fae66745363 183 {
NaotoMorita 44:7d82e63b6a86 184
NaotoMorita 19:3fae66745363 185 Matrix qout(4,1);
NaotoMorita 44:7d82e63b6a86 186 qout(1,1) = p(1,1)*q(1,1) - p(2,1)*q(2,1) - p(3,1)*q(3,1) - p(4,1)*q(4,1);
NaotoMorita 44:7d82e63b6a86 187 qout(2,1) = p(1,1)*q(2,1) + p(2,1)*q(1,1) + p(3,1)*q(4,1) - p(4,1)*q(3,1);
NaotoMorita 44:7d82e63b6a86 188 qout(3,1) = p(1,1)*q(3,1) - p(2,1)*q(4,1) + p(3,1)*q(1,1) + p(4,1)*q(2,1);
NaotoMorita 44:7d82e63b6a86 189 qout(4,1) = p(1,1)*q(4,1) + p(2,1)*q(3,1) - p(3,1)*q(2,1) + p(4,1)*q(1,1);
NaotoMorita 19:3fae66745363 190 float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qout),qout));
NaotoMorita 19:3fae66745363 191 qout *= (1.0f/ qnorm);
NaotoMorita 19:3fae66745363 192 return qout;
NaotoMorita 19:3fae66745363 193 }
NaotoMorita 19:3fae66745363 194
NaotoMorita 44:7d82e63b6a86 195 void solaESKF::computeDcm(Matrix& dcm, Matrix quat)
NaotoMorita 19:3fae66745363 196 {
NaotoMorita 19:3fae66745363 197
NaotoMorita 44:7d82e63b6a86 198 dcm(1,1) = quat(1,1)*quat(1,1) + quat(2,1)*quat(2,1) - quat(3,1)*quat(3,1) - quat(4,1)*quat(4,1);
NaotoMorita 44:7d82e63b6a86 199 dcm(1,2) = 2.0f*(quat(2,1)*quat(3,1) - quat(1,1)*quat(4,1));
NaotoMorita 44:7d82e63b6a86 200 dcm(1,3) = 2.0f*(quat(2,1)*quat(4,1) + quat(1,1)*quat(3,1));
NaotoMorita 44:7d82e63b6a86 201 dcm(2,1) = 2.0f*(quat(2,1)*quat(3,1) + quat(1,1)*quat(4,1));
NaotoMorita 44:7d82e63b6a86 202 dcm(2,2) = quat(1,1)*quat(1,1) - quat(2,1)*quat(2,1) + quat(3,1)*quat(3,1) - quat(4,1)*quat(4,1);
NaotoMorita 44:7d82e63b6a86 203 dcm(2,3) = 2.0f*(quat(3,1)*quat(4,1) - quat(1,1)*quat(2,1));
NaotoMorita 44:7d82e63b6a86 204 dcm(3,1) = 2.0f*(quat(2,1)*quat(4,1) - quat(1,1)*quat(3,1));
NaotoMorita 44:7d82e63b6a86 205 dcm(3,2) = 2.0f*(quat(3,1)*quat(4,1) + quat(1,1)*quat(2,1));
NaotoMorita 44:7d82e63b6a86 206 dcm(3,3) = quat(1,1)*quat(1,1) - quat(2,1)*quat(2,1) - quat(3,1)*quat(3,1) + quat(4,1)*quat(4,1);
NaotoMorita 19:3fae66745363 207
NaotoMorita 19:3fae66745363 208 }
NaotoMorita 19:3fae66745363 209
NaotoMorita 44:7d82e63b6a86 210 void solaESKF::setQhat(float ex,float ey,float ez)
NaotoMorita 44:7d82e63b6a86 211 {
NaotoMorita 44:7d82e63b6a86 212 float cos_z_2 = cosf(0.5f*ez);
NaotoMorita 44:7d82e63b6a86 213 float cos_y_2 = cosf(0.5f*ey);
NaotoMorita 44:7d82e63b6a86 214 float cos_x_2 = cosf(0.5f*ex);
NaotoMorita 19:3fae66745363 215
NaotoMorita 44:7d82e63b6a86 216 float sin_z_2 = sinf(0.5f*ez);
NaotoMorita 44:7d82e63b6a86 217 float sin_y_2 = sinf(0.5f*ey);
NaotoMorita 44:7d82e63b6a86 218 float sin_x_2 = sinf(0.5f*ex);
NaotoMorita 19:3fae66745363 219
NaotoMorita 19:3fae66745363 220 // and now compute quaternion
NaotoMorita 19:3fae66745363 221 qhat(1,1) = cos_z_2*cos_y_2*cos_x_2 + sin_z_2*sin_y_2*sin_x_2;
NaotoMorita 19:3fae66745363 222 qhat(2,1) = cos_z_2*cos_y_2*sin_x_2 - sin_z_2*sin_y_2*cos_x_2;
NaotoMorita 19:3fae66745363 223 qhat(3,1) = cos_z_2*sin_y_2*cos_x_2 + sin_z_2*cos_y_2*sin_x_2;
NaotoMorita 19:3fae66745363 224 qhat(4,1) = sin_z_2*cos_y_2*cos_x_2 - cos_z_2*sin_y_2*sin_x_2;
NaotoMorita 19:3fae66745363 225 }
NaotoMorita 19:3fae66745363 226
NaotoMorita 44:7d82e63b6a86 227 void solaESKF::setGravity(float gx,float gy,float gz)
NaotoMorita 23:1509648c2318 228 {
NaotoMorita 44:7d82e63b6a86 229 gravity(1,1) = gx;
NaotoMorita 44:7d82e63b6a86 230 gravity(2,1) = gy;
NaotoMorita 44:7d82e63b6a86 231 gravity(3,1) = gz;
NaotoMorita 23:1509648c2318 232 }
NaotoMorita 23:1509648c2318 233
NaotoMorita 19:3fae66745363 234
NaotoMorita 19:3fae66745363 235
NaotoMorita 44:7d82e63b6a86 236 void solaESKF::setDiag(Matrix& mat, float val){
NaotoMorita 44:7d82e63b6a86 237 for (int i = 1; i < mat.getCols()+1; i++){
NaotoMorita 44:7d82e63b6a86 238 mat(i,i) = val;
NaotoMorita 22:7d84b8bc20b4 239 }
NaotoMorita 22:7d84b8bc20b4 240 }
NaotoMorita 22:7d84b8bc20b4 241
NaotoMorita 45:df4618814803 242 void solaESKF::setBlockDiag(Matrix& mat, float val,int startIndex, int endIndex){
NaotoMorita 44:7d82e63b6a86 243 for (int i = startIndex; i < endIndex+1; i++){
NaotoMorita 44:7d82e63b6a86 244 mat(i,i) = val;
NaotoMorita 22:7d84b8bc20b4 245 }
NaotoMorita 22:7d84b8bc20b4 246 }