HAPSRG / solaESKF_Eigen

Eigen

Dependencies:   Eigen

Dependents:   optWingforHAPS_Eigen hexaTest_Eigen

solaESKF.cpp@49:0a1976eb5420, 2021-10-29 (annotated)

Committer:
NaotoMorita
Date:
Fri Oct 29 13:35:28 2021 +0000
Revision:
49:0a1976eb5420
Parent:
48:06ed39e3376e 
high order update

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