HAPSRG / solaESKF_Eigen

Eigen

Dependencies:   Eigen

Dependents:   optWingforHAPS_Eigen hexaTest_Eigen

solaESKF.cpp@46:15988dc41923, 2021-10-28 (annotated)

Committer:
NaotoMorita
Date:
Thu Oct 28 09:44:34 2021 +0000
Revision:
46:15988dc41923
Parent:
45:df4618814803
Child:
47:2467de40951f
newer

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 46:15988dc41923 8 :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)
NaotoMorita 19:3fae66745363 9 {
NaotoMorita 46:15988dc41923 10 pihat << 0.0f << 0.0f << 0.0f;
NaotoMorita 46:15988dc41923 11 vihat << 0.0f << 0.0f << 0.0f;
NaotoMorita 46:15988dc41923 12 qhat << 1.0f << 0.0f << 0.0f << 0.0f;
NaotoMorita 46:15988dc41923 13 accBias << 0.0f << 0.0f << 0.0f;
NaotoMorita 46:15988dc41923 14 gyroBias << 0.0f << 0.0f << 0.0f;
NaotoMorita 46:15988dc41923 15 gravity << 0.0f << 0.0f << 9.8f;
NaotoMorita 46:15988dc41923 16
NaotoMorita 46:15988dc41923 17 for (int i = 1; i < 19; i++){
NaotoMorita 46:15988dc41923 18 errState(i,1) = 0.0f;
NaotoMorita 46:15988dc41923 19 for (int j = 1; j < 19; j++){
NaotoMorita 46:15988dc41923 20 Phat(i,j) = 0.0f;
NaotoMorita 46:15988dc41923 21 Q(i,j) = 0.0f;
NaotoMorita 46:15988dc41923 22 }
NaotoMorita 46:15988dc41923 23 }
NaotoMorita 44:7d82e63b6a86 24
NaotoMorita 44:7d82e63b6a86 25
NaotoMorita 21:d6079def0473 26 nState = errState.getRows();
NaotoMorita 46:15988dc41923 27 setBlockDiag(Phat,0.1f,1,3);//position
NaotoMorita 46:15988dc41923 28 setBlockDiag(Phat,0.1f,4,6);//velocity
NaotoMorita 46:15988dc41923 29 setBlockDiag(Phat,0.1f,7,9);//angle error
NaotoMorita 46:15988dc41923 30 setBlockDiag(Phat,0.01f,10,12);//acc bias
NaotoMorita 46:15988dc41923 31 setBlockDiag(Phat,0.01f,13,15);//gyro bias
NaotoMorita 46:15988dc41923 32 setBlockDiag(Phat,0.00001f,16,18);//gravity
NaotoMorita 46:15988dc41923 33 setBlockDiag(Q,0.0001f,4,6);//velocity
NaotoMorita 46:15988dc41923 34 setBlockDiag(Q,0.00001f,7,9);//angle error
NaotoMorita 46:15988dc41923 35 setBlockDiag(Q,0.001f,10,12);//acc bias
NaotoMorita 46:15988dc41923 36 setBlockDiag(Q,0.001f,13,15);//gyro bias //positionとgravityはQ項なし
NaotoMorita 19:3fae66745363 37
NaotoMorita 19:3fae66745363 38
NaotoMorita 19:3fae66745363 39 }
NaotoMorita 19:3fae66745363 40
NaotoMorita 44:7d82e63b6a86 41 void solaESKF::updateNominal(Matrix acc, Matrix gyro,float att_dt)
NaotoMorita 19:3fae66745363 42 {
NaotoMorita 44:7d82e63b6a86 43 Matrix gyrom = gyro - gyroBias;
NaotoMorita 44:7d82e63b6a86 44 Matrix accm = acc - accBias;
NaotoMorita 44:7d82e63b6a86 45
NaotoMorita 44:7d82e63b6a86 46 Matrix qint(4,1);
NaotoMorita 46:15988dc41923 47 qint << 1.0f << 0.5f*gyrom(1,1)*att_dt << 0.5f*gyrom(2,1)*att_dt << 0.5f*gyrom(3,1)*att_dt;
NaotoMorita 44:7d82e63b6a86 48 qhat = quatmultiply(qhat,qint);
NaotoMorita 19:3fae66745363 49 float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qhat),qhat));
NaotoMorita 19:3fae66745363 50 qhat *= (1.0f/ qnorm);
NaotoMorita 23:1509648c2318 51
NaotoMorita 23:1509648c2318 52 Matrix dcm(3,3);
NaotoMorita 23:1509648c2318 53 computeDcm(dcm, qhat);
NaotoMorita 46:15988dc41923 54 Matrix accned = dcm*accm+gravity;
NaotoMorita 44:7d82e63b6a86 55 vihat += accned*att_dt;
NaotoMorita 44:7d82e63b6a86 56
NaotoMorita 44:7d82e63b6a86 57 pihat += vihat*att_dt+0.5f*accned*att_dt*att_dt;
NaotoMorita 19:3fae66745363 58 }
NaotoMorita 19:3fae66745363 59
NaotoMorita 44:7d82e63b6a86 60 void solaESKF::updateErrState(Matrix acc,Matrix gyro,float att_dt)
NaotoMorita 23:1509648c2318 61 {
NaotoMorita 44:7d82e63b6a86 62 Matrix gyrom = gyro - gyroBias;
NaotoMorita 44:7d82e63b6a86 63 Matrix accm = acc - accBias;
NaotoMorita 23:1509648c2318 64
NaotoMorita 23:1509648c2318 65 Matrix dcm(3,3);
NaotoMorita 23:1509648c2318 66 computeDcm(dcm, qhat);
NaotoMorita 46:15988dc41923 67 Matrix a2v = -dcm*MatrixMath::Matrixcross(accm(1,1),accm(2,1),accm(3,1))*att_dt;
NaotoMorita 46:15988dc41923 68 Matrix a2v2 = 0.5f*a2v*att_dt;
NaotoMorita 44:7d82e63b6a86 69
NaotoMorita 44:7d82e63b6a86 70 Matrix Fx(nState,nState);
NaotoMorita 44:7d82e63b6a86 71 //position
NaotoMorita 44:7d82e63b6a86 72 Fx(1,1) = 1.0f;
NaotoMorita 44:7d82e63b6a86 73 Fx(2,2) = 1.0f;
NaotoMorita 44:7d82e63b6a86 74 Fx(3,3) = 1.0f;
NaotoMorita 44:7d82e63b6a86 75 Fx(1,4) = 1.0f*att_dt;
NaotoMorita 44:7d82e63b6a86 76 Fx(2,5) = 1.0f*att_dt;
NaotoMorita 44:7d82e63b6a86 77 Fx(3,6) = 1.0f*att_dt;
NaotoMorita 46:15988dc41923 78 for (int i = 1; i < 4; i++){
NaotoMorita 46:15988dc41923 79 for (int j = 1; j < 4; j++){
NaotoMorita 46:15988dc41923 80 Fx(i,j+6) = a2v2(i,j);
NaotoMorita 46:15988dc41923 81 Fx(i,j+9) = -0.5f*dcm(i,j)*att_dt*att_dt;
NaotoMorita 46:15988dc41923 82 }
NaotoMorita 46:15988dc41923 83 Fx(i,i+15) = 0.5f*att_dt*att_dt;
NaotoMorita 46:15988dc41923 84 }
NaotoMorita 46:15988dc41923 85
NaotoMorita 44:7d82e63b6a86 86
NaotoMorita 44:7d82e63b6a86 87 //velocity
NaotoMorita 44:7d82e63b6a86 88 Fx(4,4) = 1.0f;
NaotoMorita 44:7d82e63b6a86 89 Fx(5,5) = 1.0f;
NaotoMorita 44:7d82e63b6a86 90 Fx(6,6) = 1.0f;
NaotoMorita 23:1509648c2318 91 for (int i = 1; i < 4; i++){
NaotoMorita 44:7d82e63b6a86 92 for (int j = 1; j < 4; j++){
NaotoMorita 44:7d82e63b6a86 93 Fx(i+3,j+6) = a2v(i,j);
NaotoMorita 44:7d82e63b6a86 94 Fx(i+3,j+9) = -dcm(i,j)*att_dt;
NaotoMorita 46:15988dc41923 95 Fx(i+3,j+12) = -a2v2(i,j);
NaotoMorita 23:1509648c2318 96 }
NaotoMorita 23:1509648c2318 97 }
NaotoMorita 44:7d82e63b6a86 98 Fx(4,16) = 1.0f*att_dt;
NaotoMorita 44:7d82e63b6a86 99 Fx(5,17) = 1.0f*att_dt;
NaotoMorita 44:7d82e63b6a86 100 Fx(6,18) = 1.0f*att_dt;
NaotoMorita 23:1509648c2318 101
NaotoMorita 44:7d82e63b6a86 102 //angulat error
NaotoMorita 46:15988dc41923 103 Fx(7,7) = 1.0f;
NaotoMorita 46:15988dc41923 104 Fx(8,8) = 1.0f;
NaotoMorita 46:15988dc41923 105 Fx(9,9) = 1.0f;
NaotoMorita 44:7d82e63b6a86 106 Fx(7,8) = gyrom(3,1)*att_dt;
NaotoMorita 44:7d82e63b6a86 107 Fx(7,9) = -gyrom(2,1)*att_dt;
NaotoMorita 44:7d82e63b6a86 108 Fx(8,7) = -gyrom(3,1)*att_dt;
NaotoMorita 44:7d82e63b6a86 109 Fx(8,9) = gyrom(1,1)*att_dt;
NaotoMorita 44:7d82e63b6a86 110 Fx(9,7) = gyrom(2,1)*att_dt;
NaotoMorita 44:7d82e63b6a86 111 Fx(9,8) = -gyrom(1,1)*att_dt;
NaotoMorita 44:7d82e63b6a86 112 Fx(7,13) = -1.0f*att_dt;
NaotoMorita 44:7d82e63b6a86 113 Fx(8,14) = -1.0f*att_dt;
NaotoMorita 44:7d82e63b6a86 114 Fx(9,15) = -1.0f*att_dt;
NaotoMorita 25:07ac5c6cd61c 115
NaotoMorita 44:7d82e63b6a86 116 //acc bias
NaotoMorita 44:7d82e63b6a86 117 Fx(10,10) = 1.0f;
NaotoMorita 44:7d82e63b6a86 118 Fx(11,11) = 1.0f;
NaotoMorita 44:7d82e63b6a86 119 Fx(12,12) = 1.0f;
NaotoMorita 44:7d82e63b6a86 120
NaotoMorita 44:7d82e63b6a86 121 //gyro bias
NaotoMorita 44:7d82e63b6a86 122 Fx(13,13) = 1.0f;
NaotoMorita 44:7d82e63b6a86 123 Fx(14,14) = 1.0f;
NaotoMorita 44:7d82e63b6a86 124 Fx(15,15) = 1.0f;
NaotoMorita 44:7d82e63b6a86 125
NaotoMorita 44:7d82e63b6a86 126 //gravity bias
NaotoMorita 44:7d82e63b6a86 127 Fx(16,16) = 1.0f;
NaotoMorita 44:7d82e63b6a86 128 Fx(17,17) = 1.0f;
NaotoMorita 44:7d82e63b6a86 129 Fx(18,18) = 1.0f;
NaotoMorita 44:7d82e63b6a86 130
NaotoMorita 44:7d82e63b6a86 131 errState = Fx * errState;
NaotoMorita 46:15988dc41923 132 Matrix Qi(nState,nState);
NaotoMorita 46:15988dc41923 133 for (int i = 1; i < 4; i++){
NaotoMorita 46:15988dc41923 134 Qi(i,i) = 0.0f;
NaotoMorita 46:15988dc41923 135 }
NaotoMorita 46:15988dc41923 136 for (int i = 4; i < 10; i++){
NaotoMorita 46:15988dc41923 137 Qi(i,i) = Q(i,i)*att_dt*att_dt;
NaotoMorita 46:15988dc41923 138 }
NaotoMorita 46:15988dc41923 139 for (int i = 10; i < 16; i++){
NaotoMorita 46:15988dc41923 140 Qi(i,i) = Q(i,i)*att_dt;
NaotoMorita 46:15988dc41923 141 }
NaotoMorita 46:15988dc41923 142 for (int i = 16; i < 19; i++){
NaotoMorita 46:15988dc41923 143 Qi(i,i) = 0.0f;
NaotoMorita 46:15988dc41923 144 }
NaotoMorita 46:15988dc41923 145 Phat = Fx*Phat*MatrixMath::Transpose(Fx)+Qi;
NaotoMorita 25:07ac5c6cd61c 146 }
NaotoMorita 25:07ac5c6cd61c 147
NaotoMorita 46:15988dc41923 148 void solaESKF::updateAccConstraints(Matrix acc,Matrix R)
NaotoMorita 46:15988dc41923 149 {
NaotoMorita 46:15988dc41923 150 Matrix accm = acc - accBias;
NaotoMorita 46:15988dc41923 151 Matrix tdcm(3,3);
NaotoMorita 46:15988dc41923 152 computeDcm(tdcm, qhat);
NaotoMorita 46:15988dc41923 153 tdcm = MatrixMath::Transpose(tdcm);
NaotoMorita 46:15988dc41923 154 Matrix tdcm_g = tdcm*gravity;
NaotoMorita 46:15988dc41923 155 Matrix a2v = MatrixMath::Matrixcross(tdcm_g(1,1),tdcm_g(2,1),tdcm_g(3,1));
NaotoMorita 46:15988dc41923 156
NaotoMorita 46:15988dc41923 157 Matrix H(3,nState);
NaotoMorita 46:15988dc41923 158 for (int i = 1; i < 4; i++){
NaotoMorita 46:15988dc41923 159 for (int j = 1; j < 4; j++){
NaotoMorita 46:15988dc41923 160 H(i,j+6) = a2v(i,j);
NaotoMorita 46:15988dc41923 161 H(i,j+15) = tdcm(i,j);
NaotoMorita 46:15988dc41923 162 }
NaotoMorita 46:15988dc41923 163 }
NaotoMorita 46:15988dc41923 164 H(1,10) = -1.0f;
NaotoMorita 46:15988dc41923 165 H(2,11) = -1.0f;
NaotoMorita 46:15988dc41923 166 H(3,12) = -1.0f;
NaotoMorita 46:15988dc41923 167
NaotoMorita 46:15988dc41923 168 Matrix A = H*Phat*MatrixMath::Transpose(H)+R;
NaotoMorita 46:15988dc41923 169 Matrix K = (Phat*MatrixMath::Transpose(H))*(MatrixMath::Inv(MatrixMath::Transpose(A)*A+10.0f*MatrixMath::Eye(3))*MatrixMath::Transpose(A));
NaotoMorita 46:15988dc41923 170
NaotoMorita 46:15988dc41923 171 Matrix z = -accm-tdcm*gravity;
NaotoMorita 46:15988dc41923 172 errState = K * z;
NaotoMorita 46:15988dc41923 173 Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*R*MatrixMath::Transpose(K);
NaotoMorita 46:15988dc41923 174 }
NaotoMorita 46:15988dc41923 175
NaotoMorita 46:15988dc41923 176 void solaESKF::updateGyroConstraints(Matrix gyro,Matrix R)
NaotoMorita 46:15988dc41923 177 {
NaotoMorita 46:15988dc41923 178 Matrix gyrom = gyro - gyroBias;
NaotoMorita 46:15988dc41923 179 Matrix dcm(3,3);
NaotoMorita 46:15988dc41923 180 computeDcm(dcm, qhat);
NaotoMorita 46:15988dc41923 181 Matrix a2v = -dcm*MatrixMath::Matrixcross(gyrom(1,1),gyrom(2,1),gyrom(3,1));
NaotoMorita 46:15988dc41923 182
NaotoMorita 46:15988dc41923 183 Matrix H(2,nState);
NaotoMorita 46:15988dc41923 184 for (int i = 1; i < 3; i++){
NaotoMorita 46:15988dc41923 185 for (int j = 1; j < 4; j++){
NaotoMorita 46:15988dc41923 186 H(i,j+6) = a2v(i,j);
NaotoMorita 46:15988dc41923 187 H(i,j+12) = -dcm(i,j);
NaotoMorita 46:15988dc41923 188 }
NaotoMorita 46:15988dc41923 189 }
NaotoMorita 46:15988dc41923 190
NaotoMorita 46:15988dc41923 191 Matrix A = H*Phat*MatrixMath::Transpose(H)+R;
NaotoMorita 46:15988dc41923 192 Matrix K = (Phat*MatrixMath::Transpose(H))*(MatrixMath::Inv(MatrixMath::Transpose(A)*A+10.0f*MatrixMath::Eye(2))*MatrixMath::Transpose(A));
NaotoMorita 46:15988dc41923 193
NaotoMorita 46:15988dc41923 194 Matrix z3 = -dcm*gyrom;
NaotoMorita 46:15988dc41923 195 Matrix z(2,1);
NaotoMorita 46:15988dc41923 196 z << z3(1,1) << z3(2,1);
NaotoMorita 46:15988dc41923 197 errState = K * z;
NaotoMorita 46:15988dc41923 198 Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*R*MatrixMath::Transpose(K);
NaotoMorita 46:15988dc41923 199 }
NaotoMorita 46:15988dc41923 200
NaotoMorita 46:15988dc41923 201
NaotoMorita 46:15988dc41923 202 void solaESKF::updateGPSVelocity(Matrix velgps,Matrix R)
NaotoMorita 19:3fae66745363 203 {
osaka 40:119792aa6d3b 204
NaotoMorita 43:cbc2c2d65131 205 Matrix H(3,nState);
NaotoMorita 46:15988dc41923 206 H(1,4) = 1.0f;
NaotoMorita 46:15988dc41923 207 H(2,5) = 1.0f;
NaotoMorita 46:15988dc41923 208 H(3,6) = 1.0f;
NaotoMorita 46:15988dc41923 209
NaotoMorita 46:15988dc41923 210 Matrix A = H*Phat*MatrixMath::Transpose(H)+R;
NaotoMorita 46:15988dc41923 211 Matrix K = (Phat*MatrixMath::Transpose(H))*(MatrixMath::Inv(MatrixMath::Transpose(A)*A+10.0f*MatrixMath::Eye(3))*MatrixMath::Transpose(A));
NaotoMorita 46:15988dc41923 212 Matrix z(3,1);
NaotoMorita 46:15988dc41923 213 z << velgps(1,1) - vihat(1,1) << velgps(2,1)-vihat(2,1) << velgps(3,1) - vihat(3,1);
NaotoMorita 46:15988dc41923 214 errState = K * z;
NaotoMorita 46:15988dc41923 215 //Phat = Phat-K*(H*Phat*MatrixMath::Transpose(H)+R)*MatrixMath::Transpose(K);
NaotoMorita 46:15988dc41923 216 Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*R*MatrixMath::Transpose(K);
NaotoMorita 46:15988dc41923 217 }
NaotoMorita 43:cbc2c2d65131 218
NaotoMorita 46:15988dc41923 219 void solaESKF::updateGPSPosition(Matrix posgps,Matrix R)
NaotoMorita 46:15988dc41923 220 {
NaotoMorita 46:15988dc41923 221 Matrix H(3,nState);
NaotoMorita 46:15988dc41923 222 H(1,1) = 1.0f;
NaotoMorita 46:15988dc41923 223 H(2,2) = 1.0f;
NaotoMorita 46:15988dc41923 224 H(3,3) = 1.0f;
osaka 40:119792aa6d3b 225
NaotoMorita 46:15988dc41923 226 Matrix A = H*Phat*MatrixMath::Transpose(H)+R;
NaotoMorita 46:15988dc41923 227 Matrix K = (Phat*MatrixMath::Transpose(H))*(MatrixMath::Inv(MatrixMath::Transpose(A)*A+1000.0f*MatrixMath::Eye(3))*MatrixMath::Transpose(A));
NaotoMorita 46:15988dc41923 228 //Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
NaotoMorita 43:cbc2c2d65131 229 Matrix z(3,1);
NaotoMorita 46:15988dc41923 230 z << posgps(1,1) - pihat(1,1) << posgps(2,1)-pihat(2,1) << posgps(3,1) - pihat(3,1);
NaotoMorita 46:15988dc41923 231 errState = K * z;
NaotoMorita 46:15988dc41923 232 //Phat = Phat-K*(H*Phat*MatrixMath::Transpose(H)+R)*MatrixMath::Transpose(K);
NaotoMorita 46:15988dc41923 233 Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*R*MatrixMath::Transpose(K);
NaotoMorita 19:3fae66745363 234 }
NaotoMorita 46:15988dc41923 235
NaotoMorita 46:15988dc41923 236 void solaESKF::updateGPS(Matrix posgps,Matrix velgps,Matrix R)
NaotoMorita 46:15988dc41923 237 {
NaotoMorita 46:15988dc41923 238
NaotoMorita 46:15988dc41923 239 Matrix H(6,nState);
NaotoMorita 46:15988dc41923 240 H(1,1) = 1.0f;
NaotoMorita 46:15988dc41923 241 H(2,2) = 1.0f;
NaotoMorita 46:15988dc41923 242 H(3,3) = 1.0f;
NaotoMorita 46:15988dc41923 243 H(4,4) = 1.0f;
NaotoMorita 46:15988dc41923 244 H(5,5) = 1.0f;
NaotoMorita 46:15988dc41923 245 H(6,6) = 1.0f;
NaotoMorita 46:15988dc41923 246
NaotoMorita 46:15988dc41923 247 Matrix A = H*Phat*MatrixMath::Transpose(H)+R;
NaotoMorita 46:15988dc41923 248 Matrix K = (Phat*MatrixMath::Transpose(H))*(MatrixMath::Inv(MatrixMath::Transpose(A)*A+10.0f*MatrixMath::Eye(6))*MatrixMath::Transpose(A));
NaotoMorita 46:15988dc41923 249 Matrix z(6,1);
NaotoMorita 46:15988dc41923 250 z << posgps(1,1) - pihat(1,1) << posgps(2,1)-pihat(2,1) << posgps(3,1) - pihat(3,1) << velgps(1,1) - vihat(1,1) << velgps(2,1)-vihat(2,1) << velgps(3,1) - vihat(3,1);
NaotoMorita 46:15988dc41923 251 errState = K * z;
NaotoMorita 46:15988dc41923 252 //Phat = Phat-K*(H*Phat*MatrixMath::Transpose(H)+R)*MatrixMath::Transpose(K);
NaotoMorita 46:15988dc41923 253 Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*R*MatrixMath::Transpose(K);
NaotoMorita 46:15988dc41923 254 }
NaotoMorita 46:15988dc41923 255
NaotoMorita 44:7d82e63b6a86 256 Matrix solaESKF::computeAngles()
NaotoMorita 25:07ac5c6cd61c 257 {
NaotoMorita 25:07ac5c6cd61c 258
NaotoMorita 44:7d82e63b6a86 259 Matrix euler(3,1);
NaotoMorita 44:7d82e63b6a86 260 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 261 euler(2,1) = asinf(-2.0f * (qhat(2,1)*qhat(4,1) - qhat(1,1)*qhat(3,1)));
NaotoMorita 44:7d82e63b6a86 262 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 263 return euler;
NaotoMorita 19:3fae66745363 264 }
NaotoMorita 19:3fae66745363 265
NaotoMorita 21:d6079def0473 266
NaotoMorita 44:7d82e63b6a86 267 void solaESKF::fuseErr2Nominal()
NaotoMorita 19:3fae66745363 268 {
NaotoMorita 44:7d82e63b6a86 269 //position
NaotoMorita 44:7d82e63b6a86 270 pihat(1,1) += errState(1,1);
NaotoMorita 44:7d82e63b6a86 271 pihat(2,1) += errState(2,1);
NaotoMorita 44:7d82e63b6a86 272 pihat(3,1) += errState(3,1);
NaotoMorita 44:7d82e63b6a86 273
NaotoMorita 44:7d82e63b6a86 274 //velocity
NaotoMorita 44:7d82e63b6a86 275 vihat(1,1) += errState(4,1);
NaotoMorita 44:7d82e63b6a86 276 vihat(2,1) += errState(5,1);
NaotoMorita 44:7d82e63b6a86 277 vihat(3,1) += errState(6,1);
NaotoMorita 44:7d82e63b6a86 278
NaotoMorita 44:7d82e63b6a86 279 //angle error
NaotoMorita 19:3fae66745363 280 Matrix qerr(4,1);
NaotoMorita 46:15988dc41923 281 qerr << 1.0f << 0.5f*errState(7,1) << 0.5f*errState(8,1) << 0.5f*errState(9,1);
NaotoMorita 19:3fae66745363 282 qhat = quatmultiply(qhat, qerr);
NaotoMorita 19:3fae66745363 283 float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qhat),qhat));
NaotoMorita 19:3fae66745363 284 qhat *= (1.0f/ qnorm);
NaotoMorita 23:1509648c2318 285
NaotoMorita 44:7d82e63b6a86 286 //acc bias
NaotoMorita 44:7d82e63b6a86 287 accBias(1,1) += errState(10,1);
NaotoMorita 44:7d82e63b6a86 288 accBias(2,1) += errState(11,1);
NaotoMorita 44:7d82e63b6a86 289 accBias(3,1) += errState(12,1);
NaotoMorita 44:7d82e63b6a86 290
NaotoMorita 44:7d82e63b6a86 291 //gyro bias
NaotoMorita 44:7d82e63b6a86 292 gyroBias(1,1) += errState(13,1);
NaotoMorita 44:7d82e63b6a86 293 gyroBias(2,1) += errState(14,1);
NaotoMorita 44:7d82e63b6a86 294 gyroBias(3,1) += errState(15,1);
NaotoMorita 44:7d82e63b6a86 295
NaotoMorita 44:7d82e63b6a86 296 //gravity bias
NaotoMorita 44:7d82e63b6a86 297 gravity(1,1) += errState(16,1);
NaotoMorita 44:7d82e63b6a86 298 gravity(2,1) += errState(17,1);
NaotoMorita 44:7d82e63b6a86 299 gravity(3,1) += errState(18,1);
NaotoMorita 44:7d82e63b6a86 300
NaotoMorita 19:3fae66745363 301 }
NaotoMorita 19:3fae66745363 302
NaotoMorita 44:7d82e63b6a86 303 Matrix solaESKF::quatmultiply(Matrix p, Matrix q)
NaotoMorita 19:3fae66745363 304 {
NaotoMorita 44:7d82e63b6a86 305
NaotoMorita 19:3fae66745363 306 Matrix qout(4,1);
NaotoMorita 44:7d82e63b6a86 307 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 308 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 309 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 310 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 311 float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qout),qout));
NaotoMorita 19:3fae66745363 312 qout *= (1.0f/ qnorm);
NaotoMorita 19:3fae66745363 313 return qout;
NaotoMorita 19:3fae66745363 314 }
NaotoMorita 19:3fae66745363 315
NaotoMorita 44:7d82e63b6a86 316 void solaESKF::computeDcm(Matrix& dcm, Matrix quat)
NaotoMorita 19:3fae66745363 317 {
NaotoMorita 19:3fae66745363 318
NaotoMorita 44:7d82e63b6a86 319 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 320 dcm(1,2) = 2.0f*(quat(2,1)*quat(3,1) - quat(1,1)*quat(4,1));
NaotoMorita 44:7d82e63b6a86 321 dcm(1,3) = 2.0f*(quat(2,1)*quat(4,1) + quat(1,1)*quat(3,1));
NaotoMorita 44:7d82e63b6a86 322 dcm(2,1) = 2.0f*(quat(2,1)*quat(3,1) + quat(1,1)*quat(4,1));
NaotoMorita 44:7d82e63b6a86 323 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 324 dcm(2,3) = 2.0f*(quat(3,1)*quat(4,1) - quat(1,1)*quat(2,1));
NaotoMorita 44:7d82e63b6a86 325 dcm(3,1) = 2.0f*(quat(2,1)*quat(4,1) - quat(1,1)*quat(3,1));
NaotoMorita 44:7d82e63b6a86 326 dcm(3,2) = 2.0f*(quat(3,1)*quat(4,1) + quat(1,1)*quat(2,1));
NaotoMorita 44:7d82e63b6a86 327 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 328
NaotoMorita 19:3fae66745363 329 }
NaotoMorita 19:3fae66745363 330
NaotoMorita 44:7d82e63b6a86 331 void solaESKF::setQhat(float ex,float ey,float ez)
NaotoMorita 44:7d82e63b6a86 332 {
NaotoMorita 44:7d82e63b6a86 333 float cos_z_2 = cosf(0.5f*ez);
NaotoMorita 44:7d82e63b6a86 334 float cos_y_2 = cosf(0.5f*ey);
NaotoMorita 44:7d82e63b6a86 335 float cos_x_2 = cosf(0.5f*ex);
NaotoMorita 19:3fae66745363 336
NaotoMorita 44:7d82e63b6a86 337 float sin_z_2 = sinf(0.5f*ez);
NaotoMorita 44:7d82e63b6a86 338 float sin_y_2 = sinf(0.5f*ey);
NaotoMorita 44:7d82e63b6a86 339 float sin_x_2 = sinf(0.5f*ex);
NaotoMorita 19:3fae66745363 340
NaotoMorita 19:3fae66745363 341 // and now compute quaternion
NaotoMorita 19:3fae66745363 342 qhat(1,1) = cos_z_2*cos_y_2*cos_x_2 + sin_z_2*sin_y_2*sin_x_2;
NaotoMorita 19:3fae66745363 343 qhat(2,1) = cos_z_2*cos_y_2*sin_x_2 - sin_z_2*sin_y_2*cos_x_2;
NaotoMorita 19:3fae66745363 344 qhat(3,1) = cos_z_2*sin_y_2*cos_x_2 + sin_z_2*cos_y_2*sin_x_2;
NaotoMorita 19:3fae66745363 345 qhat(4,1) = sin_z_2*cos_y_2*cos_x_2 - cos_z_2*sin_y_2*sin_x_2;
NaotoMorita 19:3fae66745363 346 }
NaotoMorita 19:3fae66745363 347
NaotoMorita 44:7d82e63b6a86 348 void solaESKF::setGravity(float gx,float gy,float gz)
NaotoMorita 23:1509648c2318 349 {
NaotoMorita 44:7d82e63b6a86 350 gravity(1,1) = gx;
NaotoMorita 44:7d82e63b6a86 351 gravity(2,1) = gy;
NaotoMorita 44:7d82e63b6a86 352 gravity(3,1) = gz;
NaotoMorita 23:1509648c2318 353 }
NaotoMorita 23:1509648c2318 354
NaotoMorita 19:3fae66745363 355
NaotoMorita 19:3fae66745363 356
NaotoMorita 44:7d82e63b6a86 357 void solaESKF::setDiag(Matrix& mat, float val){
NaotoMorita 44:7d82e63b6a86 358 for (int i = 1; i < mat.getCols()+1; i++){
NaotoMorita 44:7d82e63b6a86 359 mat(i,i) = val;
NaotoMorita 22:7d84b8bc20b4 360 }
NaotoMorita 22:7d84b8bc20b4 361 }
NaotoMorita 22:7d84b8bc20b4 362
NaotoMorita 45:df4618814803 363 void solaESKF::setBlockDiag(Matrix& mat, float val,int startIndex, int endIndex){
NaotoMorita 44:7d82e63b6a86 364 for (int i = startIndex; i < endIndex+1; i++){
NaotoMorita 44:7d82e63b6a86 365 mat(i,i) = val;
NaotoMorita 22:7d84b8bc20b4 366 }
NaotoMorita 22:7d84b8bc20b4 367 }