HAPSRG / solaESKF_Eigen

Eigen

Dependencies:   Eigen

Dependents:   optWingforHAPS_Eigen hexaTest_Eigen

solaESKF.cpp@48:06ed39e3376e, 2021-10-29 (annotated)

Committer:
NaotoMorita
Date:
Fri Oct 29 13:33:25 2021 +0000
Revision:
48:06ed39e3376e
Parent:
47:2467de40951f
Child:
49:0a1976eb5420
high order propagate

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