HAPSRG / solaESKF_Eigen

Eigen

Dependencies:   Eigen

Dependents:   optWingforHAPS_Eigen hexaTest_Eigen

ScErrStateEKF.cpp@22:7d84b8bc20b4, 2021-08-10 (annotated)

Committer:
NaotoMorita
Date:
Tue Aug 10 08:29:19 2021 +0000
Revision:
22:7d84b8bc20b4
Parent:
21:d6079def0473
Child:
23:1509648c2318
async

Who changed what in which revision?

UserRevisionLine numberNew contents of line
NaotoMorita 19:3fae66745363 1 #include "ScErrStateEKF.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 #include "Vector3.hpp"
NaotoMorita 19:3fae66745363 6
NaotoMorita 19:3fae66745363 7
NaotoMorita 19:3fae66745363 8 using namespace std;
NaotoMorita 19:3fae66745363 9
NaotoMorita 19:3fae66745363 10 ScErrStateEKF::ScErrStateEKF()
NaotoMorita 22:7d84b8bc20b4 11 :qhat(4,1), errState(9,1), Phat(9,9), Q(9,9), Ra(3,3), Rm(3,3), Qab(3,3),Rgsc(2,2),accBias(0.0f,0.0f,0.0f),gyroBias(0.0f,0.0f,0.0f)
NaotoMorita 19:3fae66745363 12 {
NaotoMorita 21:d6079def0473 13 nState = errState.getRows();
NaotoMorita 19:3fae66745363 14 qhat << 1.0f << 0.0f << 0.0f << 0.0f;
NaotoMorita 19:3fae66745363 15
NaotoMorita 21:d6079def0473 16 setDiag(Phat,0.1f);
NaotoMorita 21:d6079def0473 17 setQqerr(0.000005f);
NaotoMorita 21:d6079def0473 18 setQgbias(0.00001f);
NaotoMorita 21:d6079def0473 19 setQabias(0.006f);
NaotoMorita 21:d6079def0473 20 //setQv(0.000005f);
NaotoMorita 19:3fae66745363 21
NaotoMorita 20:37d3c3ee36e9 22 //加速度の観測
NaotoMorita 21:d6079def0473 23 setDiag(Ra,1.0f);
NaotoMorita 21:d6079def0473 24 setDiag(Qab,120.0f);
NaotoMorita 20:37d3c3ee36e9 25
NaotoMorita 20:37d3c3ee36e9 26 //ジャイロバイアスに関する制約
NaotoMorita 22:7d84b8bc20b4 27 setDiag(Rgsc,500.0f);
NaotoMorita 20:37d3c3ee36e9 28
NaotoMorita 21:d6079def0473 29 //機体軸速度に関する制約
NaotoMorita 21:d6079def0473 30 //setDiag(Rvsc,100.0f);
NaotoMorita 19:3fae66745363 31
NaotoMorita 21:d6079def0473 32 setDiag(Rm,5.0f);
NaotoMorita 19:3fae66745363 33
NaotoMorita 19:3fae66745363 34 for(int i = 0; i<10;i++){
NaotoMorita 19:3fae66745363 35 histffunc[i] = 0.0f;
NaotoMorita 19:3fae66745363 36 }
NaotoMorita 19:3fae66745363 37 histffuncindex = 0 ;
NaotoMorita 19:3fae66745363 38 sigma2a = 0.000020f;
NaotoMorita 19:3fae66745363 39
NaotoMorita 19:3fae66745363 40
NaotoMorita 19:3fae66745363 41 }
NaotoMorita 19:3fae66745363 42
NaotoMorita 22:7d84b8bc20b4 43 void ScErrStateEKF::updateNominal(Vector3 gyro, float att_dt)
NaotoMorita 19:3fae66745363 44 {
NaotoMorita 19:3fae66745363 45 gyro -= gyroBias;
NaotoMorita 19:3fae66745363 46 Matrix A(4,4);
NaotoMorita 19:3fae66745363 47 A << 0.0f << -0.5f*gyro.x <<-0.5f*gyro.y <<-0.5f*gyro.z
NaotoMorita 19:3fae66745363 48 << 0.5f*gyro.x << 0.0f << 0.5f*gyro.z <<-0.5f*gyro.y
NaotoMorita 19:3fae66745363 49 << 0.5f*gyro.y << -0.5f*gyro.z << 0.0f << 0.5f*gyro.x
NaotoMorita 19:3fae66745363 50 << 0.5f*gyro.z << 0.5f*gyro.y <<-0.5f*gyro.x << 0.0f ;
NaotoMorita 19:3fae66745363 51
NaotoMorita 20:37d3c3ee36e9 52 Matrix phi = MatrixMath::Eye(4) + A * att_dt + (0.5f*att_dt*att_dt) * A * A;
NaotoMorita 19:3fae66745363 53 qhat = phi * qhat;
NaotoMorita 19:3fae66745363 54 float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qhat),qhat));
NaotoMorita 19:3fae66745363 55 qhat *= (1.0f/ qnorm);
NaotoMorita 19:3fae66745363 56 }
NaotoMorita 19:3fae66745363 57
NaotoMorita 20:37d3c3ee36e9 58 void ScErrStateEKF::setQqerr(float val){
NaotoMorita 20:37d3c3ee36e9 59 Q(1,1) = val;
NaotoMorita 20:37d3c3ee36e9 60 Q(2,2) = val;
NaotoMorita 20:37d3c3ee36e9 61 Q(3,3) = val;
NaotoMorita 20:37d3c3ee36e9 62 }
NaotoMorita 20:37d3c3ee36e9 63
NaotoMorita 20:37d3c3ee36e9 64 void ScErrStateEKF::setQgbias(float val){
NaotoMorita 20:37d3c3ee36e9 65 Q(4,4) = val;
NaotoMorita 20:37d3c3ee36e9 66 Q(5,5) = val;
NaotoMorita 20:37d3c3ee36e9 67 Q(6,6) = val;
NaotoMorita 20:37d3c3ee36e9 68 }
NaotoMorita 20:37d3c3ee36e9 69 void ScErrStateEKF::setQabias(float val){
NaotoMorita 20:37d3c3ee36e9 70 Q(7,7) = val;
NaotoMorita 20:37d3c3ee36e9 71 Q(8,8) = val;
NaotoMorita 20:37d3c3ee36e9 72 Q(9,9) = val;
NaotoMorita 20:37d3c3ee36e9 73 }
NaotoMorita 20:37d3c3ee36e9 74
NaotoMorita 21:d6079def0473 75
NaotoMorita 20:37d3c3ee36e9 76 void ScErrStateEKF::setQab(float val){
NaotoMorita 21:d6079def0473 77 setDiag(Qab,val);
NaotoMorita 20:37d3c3ee36e9 78 }
NaotoMorita 20:37d3c3ee36e9 79
NaotoMorita 20:37d3c3ee36e9 80 void ScErrStateEKF::setRsoftconst(float Vgsc,float Vvsc){
NaotoMorita 21:d6079def0473 81 setDiag(Rgsc,Vgsc);
NaotoMorita 22:7d84b8bc20b4 82 //setDiag(Rvsc,Vvsc);
NaotoMorita 20:37d3c3ee36e9 83 }
NaotoMorita 20:37d3c3ee36e9 84
NaotoMorita 21:d6079def0473 85 void ScErrStateEKF::setDiag(Matrix& mat, float val){
NaotoMorita 21:d6079def0473 86 for (int i = 1; i < mat.getCols()+1; i++){
NaotoMorita 21:d6079def0473 87 mat(i,i) = val;
NaotoMorita 21:d6079def0473 88 }
NaotoMorita 21:d6079def0473 89 }
NaotoMorita 20:37d3c3ee36e9 90
NaotoMorita 22:7d84b8bc20b4 91 void ScErrStateEKF::updateErrState(Vector3 gyro, float att_dt)
NaotoMorita 19:3fae66745363 92 {
NaotoMorita 19:3fae66745363 93 gyro -= gyroBias;
NaotoMorita 21:d6079def0473 94 Matrix A(nState,nState);
NaotoMorita 19:3fae66745363 95 A(1,2) = gyro.z;
NaotoMorita 19:3fae66745363 96 A(1,3) = -gyro.y;
NaotoMorita 19:3fae66745363 97 A(2,1) = -gyro.z;
NaotoMorita 19:3fae66745363 98 A(2,3) = gyro.x;
NaotoMorita 19:3fae66745363 99 A(3,1) = gyro.y;
NaotoMorita 19:3fae66745363 100 A(3,2) = -gyro.x;
NaotoMorita 19:3fae66745363 101 A(1,4) = -0.5f;
NaotoMorita 19:3fae66745363 102 A(2,5) = -0.5f;
NaotoMorita 19:3fae66745363 103 A(3,6) = -0.5f;
NaotoMorita 19:3fae66745363 104
NaotoMorita 21:d6079def0473 105 Matrix phi = MatrixMath::Eye(nState) + A * att_dt + (0.5f*att_dt*att_dt) * A * A;
NaotoMorita 19:3fae66745363 106 Matrix Qd = Q * att_dt + 0.5f*A*Q * att_dt * att_dt+ 0.5f*Q*MatrixMath::Transpose(A) * att_dt * att_dt;
NaotoMorita 19:3fae66745363 107 errState = phi * errState;
NaotoMorita 19:3fae66745363 108 Phat = phi*Phat*MatrixMath::Transpose(phi)+Qd;
NaotoMorita 19:3fae66745363 109 }
NaotoMorita 19:3fae66745363 110
NaotoMorita 19:3fae66745363 111 void ScErrStateEKF::updateAccMeasures(Vector3 acc, Vector3 accref)
NaotoMorita 19:3fae66745363 112 {
NaotoMorita 21:d6079def0473 113 acc -= accBias;
NaotoMorita 20:37d3c3ee36e9 114 Matrix dcm(3,3);
NaotoMorita 20:37d3c3ee36e9 115 computeDcm(dcm, qhat);
NaotoMorita 20:37d3c3ee36e9 116 Vector3 gvec(dcm(1,3)*accref.z, dcm(2,3)*accref.z, dcm(3,3)*accref.z);
NaotoMorita 21:d6079def0473 117 Matrix H(3,nState);
NaotoMorita 20:37d3c3ee36e9 118 H(1,2) = -2.0f*gvec.z;
NaotoMorita 20:37d3c3ee36e9 119 H(1,3) = 2.0f*gvec.y;
NaotoMorita 20:37d3c3ee36e9 120 H(2,1) = 2.0f*gvec.z;
NaotoMorita 20:37d3c3ee36e9 121 H(2,3) = -2.0f*gvec.x;
NaotoMorita 20:37d3c3ee36e9 122 H(3,1) = -2.0f*gvec.y;
NaotoMorita 20:37d3c3ee36e9 123 H(3,2) = 2.0f*gvec.x;
NaotoMorita 20:37d3c3ee36e9 124 H(1,7) = 1.0f;
NaotoMorita 20:37d3c3ee36e9 125 H(2,8) = 1.0f;
NaotoMorita 20:37d3c3ee36e9 126 H(3,9) = 1.0f;
NaotoMorita 20:37d3c3ee36e9 127 Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+Ra+Qab);
NaotoMorita 21:d6079def0473 128 Matrix z = MatrixMath::Vector2mat(acc)-dcm*MatrixMath::Vector2mat(accref);
NaotoMorita 20:37d3c3ee36e9 129 Matrix corrVal = K * (z-H*errState);
NaotoMorita 20:37d3c3ee36e9 130 errState = errState + corrVal;
NaotoMorita 21:d6079def0473 131 Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*(Ra+Qab)*MatrixMath::Transpose(K);
NaotoMorita 20:37d3c3ee36e9 132 }
NaotoMorita 20:37d3c3ee36e9 133
NaotoMorita 20:37d3c3ee36e9 134 void ScErrStateEKF::updateGyroBiasConstraints(Vector3 gyro)
NaotoMorita 20:37d3c3ee36e9 135 {
NaotoMorita 21:d6079def0473 136 gyro -= gyroBias;
NaotoMorita 21:d6079def0473 137
NaotoMorita 20:37d3c3ee36e9 138 Matrix dcm(3,3);
NaotoMorita 20:37d3c3ee36e9 139 computeDcm(dcm, qhat);
NaotoMorita 22:7d84b8bc20b4 140 Matrix qeterm = 2.0f*MatrixMath::Transpose(dcm)*MatrixMath::Matrixcross(gyro.x,gyro.y,gyro.z);
NaotoMorita 21:d6079def0473 141 Matrix H(2,nState);
NaotoMorita 21:d6079def0473 142 for(int i = 1; i<3; i++){
NaotoMorita 21:d6079def0473 143 for(int j = 1;j<4;j++){
NaotoMorita 21:d6079def0473 144 H(i,j) = qeterm(i,j);
NaotoMorita 21:d6079def0473 145 }
NaotoMorita 21:d6079def0473 146 }
NaotoMorita 21:d6079def0473 147 H(1,4) = 1.0f*(dcm(1,1));
NaotoMorita 21:d6079def0473 148 H(1,5) = 1.0f*(dcm(2,1));
NaotoMorita 21:d6079def0473 149 H(1,6) = 1.0f*(dcm(3,1));
NaotoMorita 21:d6079def0473 150 H(2,4) = 1.0f*(dcm(1,2));
NaotoMorita 21:d6079def0473 151 H(2,5) = 1.0f*(dcm(2,2));
NaotoMorita 21:d6079def0473 152 H(2,6) = 1.0f*(dcm(3,2));
NaotoMorita 20:37d3c3ee36e9 153 Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+Rgsc);
NaotoMorita 20:37d3c3ee36e9 154 Matrix z(2,1);
NaotoMorita 21:d6079def0473 155 z <<dcm(1,1)*gyro.x+dcm(2,1)*gyro.y+dcm(3,1)*gyro.z<< dcm(1,2)*gyro.x+dcm(2,2)*gyro.y+dcm(3,2)*gyro.z;
NaotoMorita 20:37d3c3ee36e9 156 Matrix corrVal = K * (z-H*errState);
NaotoMorita 20:37d3c3ee36e9 157 errState = errState + corrVal;
NaotoMorita 21:d6079def0473 158 Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*(Rgsc)*MatrixMath::Transpose(K);
NaotoMorita 20:37d3c3ee36e9 159 }
NaotoMorita 20:37d3c3ee36e9 160
NaotoMorita 21:d6079def0473 161
NaotoMorita 21:d6079def0473 162
NaotoMorita 19:3fae66745363 163 void ScErrStateEKF::updateStaticAccMeasures(Vector3 acc, Vector3 accref)
NaotoMorita 19:3fae66745363 164 {
NaotoMorita 19:3fae66745363 165 Matrix dcm(3,3);
NaotoMorita 19:3fae66745363 166 computeDcm(dcm, qhat);
NaotoMorita 19:3fae66745363 167 Vector3 gvec(dcm(1,3)*accref.z, dcm(2,3)*accref.z, dcm(3,3)*accref.z);
NaotoMorita 21:d6079def0473 168 Matrix H(3,nState);
NaotoMorita 19:3fae66745363 169 H(1,2) = -2.0f*gvec.z;
NaotoMorita 19:3fae66745363 170 H(1,3) = 2.0f*gvec.y;
NaotoMorita 19:3fae66745363 171 H(2,1) = 2.0f*gvec.z;
NaotoMorita 19:3fae66745363 172 H(2,3) = -2.0f*gvec.x;
NaotoMorita 19:3fae66745363 173 H(3,1) = -2.0f*gvec.y;
NaotoMorita 19:3fae66745363 174 H(3,2) = 2.0f*gvec.x;
NaotoMorita 19:3fae66745363 175 Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+Ra*100.0f);
NaotoMorita 19:3fae66745363 176 Matrix accmat(3,1);
NaotoMorita 19:3fae66745363 177 accmat << acc.x << acc.y << acc.z;
NaotoMorita 19:3fae66745363 178 Matrix gref(3,1);
NaotoMorita 19:3fae66745363 179 gref << 0.0f << 0.0f << accref.z;
NaotoMorita 19:3fae66745363 180 Matrix z = accmat-dcm*gref;
NaotoMorita 19:3fae66745363 181 errState = errState + K * (z-H*errState);
NaotoMorita 21:d6079def0473 182 Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*(Ra*100.0f)*MatrixMath::Transpose(K);
NaotoMorita 19:3fae66745363 183 }
NaotoMorita 19:3fae66745363 184
NaotoMorita 19:3fae66745363 185
NaotoMorita 19:3fae66745363 186
NaotoMorita 19:3fae66745363 187 void ScErrStateEKF::updateMagMeasures(Vector3 mag)
NaotoMorita 19:3fae66745363 188 {
NaotoMorita 19:3fae66745363 189 //mag = mag/mag.Norm();
NaotoMorita 19:3fae66745363 190 Matrix dcm(3,3);
NaotoMorita 19:3fae66745363 191 computeDcm(dcm, qhat);
NaotoMorita 19:3fae66745363 192
NaotoMorita 19:3fae66745363 193 Matrix magvec(3,1);
NaotoMorita 19:3fae66745363 194 magvec(1,1) = mag.x;
NaotoMorita 19:3fae66745363 195 magvec(2,1) = mag.y;
NaotoMorita 19:3fae66745363 196 magvec(3,1) = mag.z;
NaotoMorita 19:3fae66745363 197
NaotoMorita 19:3fae66745363 198 Matrix magnedvec = MatrixMath::Transpose(dcm)*magvec;
NaotoMorita 19:3fae66745363 199 Matrix magrefmod(3,1);
NaotoMorita 19:3fae66745363 200 magrefmod(1,1) = sqrt(magnedvec(1,1)*magnedvec(1,1)+magnedvec(2,1)*magnedvec(2,1));
NaotoMorita 19:3fae66745363 201 magrefmod(2,1) = 0.0f;
NaotoMorita 19:3fae66745363 202 magrefmod(3,1) = magnedvec(3,1);
NaotoMorita 19:3fae66745363 203
NaotoMorita 19:3fae66745363 204 Matrix mvec = dcm*magrefmod ;
NaotoMorita 21:d6079def0473 205 Matrix H(3,nState);
NaotoMorita 19:3fae66745363 206 H(1,2) = -2.0f*magvec(3,1);
NaotoMorita 19:3fae66745363 207 H(1,3) = 2.0f*magvec(2,1);
NaotoMorita 19:3fae66745363 208 H(2,1) = 2.0f*magvec(3,1);
NaotoMorita 19:3fae66745363 209 H(2,3) = -2.0f*magvec(1,1);
NaotoMorita 19:3fae66745363 210 H(3,1) = -2.0f*magvec(2,1);
NaotoMorita 19:3fae66745363 211 H(3,2) = 2.0f*magvec(1,1);
NaotoMorita 21:d6079def0473 212 Matrix Pm(nState,nState);
NaotoMorita 19:3fae66745363 213 for(int i = 1; i<4; i++){
NaotoMorita 19:3fae66745363 214 for(int j = 1;j<4;j++){
NaotoMorita 19:3fae66745363 215 Pm(i,j) = Phat(i,j);
NaotoMorita 19:3fae66745363 216 }
NaotoMorita 19:3fae66745363 217 }
NaotoMorita 19:3fae66745363 218 Matrix r3(3,1);
NaotoMorita 19:3fae66745363 219 r3 << dcm(1,3)<< dcm(2,3) << dcm(3,3);
NaotoMorita 19:3fae66745363 220 Matrix kpart = r3*MatrixMath::Transpose(r3);
NaotoMorita 21:d6079def0473 221 Matrix Kmod(nState,nState);
NaotoMorita 19:3fae66745363 222 for(int i = 1; i<4; i++){
NaotoMorita 19:3fae66745363 223 for(int j = 1;j<4;j++){
NaotoMorita 19:3fae66745363 224 Kmod(i,j) = kpart(i,j);
NaotoMorita 19:3fae66745363 225 }
NaotoMorita 19:3fae66745363 226 }
NaotoMorita 19:3fae66745363 227
NaotoMorita 19:3fae66745363 228 Matrix K = Kmod*(Pm*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Pm*MatrixMath::Transpose(H)+Rm);
NaotoMorita 19:3fae66745363 229 Matrix z = magvec-mvec;
NaotoMorita 19:3fae66745363 230 errState = errState + K * (z-H*errState);
NaotoMorita 21:d6079def0473 231 Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*(Rm)*MatrixMath::Transpose(K);
NaotoMorita 19:3fae66745363 232 }
NaotoMorita 19:3fae66745363 233
NaotoMorita 19:3fae66745363 234 void ScErrStateEKF::resetBias()
NaotoMorita 19:3fae66745363 235 {
NaotoMorita 19:3fae66745363 236 gyroBias.x = gyroBias.x + errState(4,1)*1.0f;
NaotoMorita 19:3fae66745363 237 gyroBias.y = gyroBias.y + errState(5,1)*1.0f;
NaotoMorita 19:3fae66745363 238 gyroBias.z = gyroBias.z + errState(6,1)*1.0f;
NaotoMorita 19:3fae66745363 239 errState(4,1) = 0.0f;
NaotoMorita 19:3fae66745363 240 errState(5,1) = 0.0f;
NaotoMorita 19:3fae66745363 241 errState(6,1) = 0.0f;
NaotoMorita 20:37d3c3ee36e9 242 accBias.x = accBias.x + errState(7,1);
NaotoMorita 20:37d3c3ee36e9 243 accBias.y = accBias.y + errState(8,1);
NaotoMorita 20:37d3c3ee36e9 244 accBias.z = accBias.z + errState(9,1);
NaotoMorita 20:37d3c3ee36e9 245 errState(7,1) = 0.0f;
NaotoMorita 20:37d3c3ee36e9 246 errState(8,1) = 0.0f;
NaotoMorita 20:37d3c3ee36e9 247 errState(9,1) = 0.0f;
NaotoMorita 19:3fae66745363 248 }
NaotoMorita 19:3fae66745363 249
NaotoMorita 19:3fae66745363 250 void ScErrStateEKF::computeAngles(Vector3& rpy,Vector3 rpy_align)
NaotoMorita 19:3fae66745363 251 {
NaotoMorita 19:3fae66745363 252 Matrix qerr(4,1);
NaotoMorita 19:3fae66745363 253 qerr << 1.0f << errState(1,1) << errState(2,1) << errState(3,1);
NaotoMorita 19:3fae66745363 254 //float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qerr),qerr));
NaotoMorita 19:3fae66745363 255 //qerr *= (1.0f/ qnorm);
NaotoMorita 19:3fae66745363 256 Matrix qest = quatmultiply(qhat, qerr);
NaotoMorita 19:3fae66745363 257 float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qest),qest));
NaotoMorita 19:3fae66745363 258 qest *= (1.0f/ qnorm);
NaotoMorita 19:3fae66745363 259 float q0 = qest( 1, 1 );
NaotoMorita 19:3fae66745363 260 float q1 = qest( 2, 1 );
NaotoMorita 19:3fae66745363 261 float q2 = qest( 3, 1 );
NaotoMorita 19:3fae66745363 262 float q3 = qest( 4, 1 );
NaotoMorita 19:3fae66745363 263 rpy.x = atan2f(q0*q1 + q2*q3, 0.5f - q1*q1 - q2*q2)-rpy_align.x;
NaotoMorita 19:3fae66745363 264 rpy.y = asinf(-2.0f * (q1*q3 - q0*q2))-rpy_align.y;
NaotoMorita 19:3fae66745363 265 rpy.z = atan2f(q1*q2 + q0*q3, 0.5f - q2*q2 - q3*q3);
NaotoMorita 19:3fae66745363 266
NaotoMorita 19:3fae66745363 267 }
NaotoMorita 19:3fae66745363 268
NaotoMorita 21:d6079def0473 269
NaotoMorita 21:d6079def0473 270 void ScErrStateEKF::fuseErr2Nominal()
NaotoMorita 19:3fae66745363 271 {
NaotoMorita 19:3fae66745363 272 Matrix qerr(4,1);
NaotoMorita 19:3fae66745363 273 qerr << 1.0f << errState(1,1) << errState(2,1) << errState(3,1);
NaotoMorita 19:3fae66745363 274 //float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qerr),qerr));
NaotoMorita 19:3fae66745363 275 //qerr *= (1.0f/ qnorm);
NaotoMorita 19:3fae66745363 276 qhat = quatmultiply(qhat, qerr);
NaotoMorita 19:3fae66745363 277 errState(1,1) = 0.0f;
NaotoMorita 19:3fae66745363 278 errState(2,1) = 0.0f;
NaotoMorita 19:3fae66745363 279 errState(3,1) = 0.0f;
NaotoMorita 19:3fae66745363 280 float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qhat),qhat));
NaotoMorita 19:3fae66745363 281 qhat *= (1.0f/ qnorm);
NaotoMorita 19:3fae66745363 282 }
NaotoMorita 19:3fae66745363 283
NaotoMorita 19:3fae66745363 284 Matrix ScErrStateEKF::quatmultiply(Matrix q, Matrix r)
NaotoMorita 19:3fae66745363 285 {
NaotoMorita 19:3fae66745363 286 Matrix qout(4,1);
NaotoMorita 19:3fae66745363 287 qout(1,1) = q(1,1)*r(1,1) - q(2,1)*r(2,1) - q(3,1)*r(3,1) - q(4,1)*r(4,1);
NaotoMorita 19:3fae66745363 288 qout(2,1) = q(1,1)*r(2,1) + r(1,1)*q(2,1) + q(3,1)*r(4,1)-q(4,1)*r(3,1);
NaotoMorita 19:3fae66745363 289 qout(3,1) = q(1,1)*r(3,1) + r(1,1)*q(3,1) + q(4,1)*r(2,1)-q(2,1)*r(4,1);
NaotoMorita 19:3fae66745363 290 qout(4,1) = q(1,1)*r(4,1) + r(1,1)*q(4,1) + q(2,1)*r(3,1)-q(3,1)*r(2,1);
NaotoMorita 19:3fae66745363 291 float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qout),qout));
NaotoMorita 19:3fae66745363 292 qout *= (1.0f/ qnorm);
NaotoMorita 19:3fae66745363 293 return qout;
NaotoMorita 19:3fae66745363 294 }
NaotoMorita 19:3fae66745363 295
NaotoMorita 19:3fae66745363 296 void ScErrStateEKF::computeDcm(Matrix& dcm, Matrix quat)
NaotoMorita 19:3fae66745363 297 {
NaotoMorita 19:3fae66745363 298
NaotoMorita 19:3fae66745363 299 float q0 = quat( 1, 1 );
NaotoMorita 19:3fae66745363 300 float q1 = quat( 2, 1 );
NaotoMorita 19:3fae66745363 301 float q2 = quat( 3, 1 );
NaotoMorita 19:3fae66745363 302 float q3 = quat( 4, 1 );
NaotoMorita 19:3fae66745363 303
NaotoMorita 19:3fae66745363 304 dcm(1,1) = q0*q0 + q1*q1 - q2*q2 - q3*q3;
NaotoMorita 19:3fae66745363 305 dcm(1,2) = 2*(q1*q2 + q0*q3);
NaotoMorita 19:3fae66745363 306 dcm(1,3) = 2*(q1*q3 - q0*q2);
NaotoMorita 19:3fae66745363 307 dcm(2,1) = 2*(q1*q2 - q0*q3);
NaotoMorita 19:3fae66745363 308 dcm(2,2) = q0*q0 - q1*q1 + q2*q2 - q3*q3;
NaotoMorita 19:3fae66745363 309 dcm(2,3) = 2*(q2*q3 + q0*q1);
NaotoMorita 19:3fae66745363 310 dcm(3,1) = 2*(q1*q3 + q0*q2);
NaotoMorita 19:3fae66745363 311 dcm(3,2) = 2*(q2*q3 - q0*q1);
NaotoMorita 19:3fae66745363 312 dcm(3,3) = q0*q0 - q1*q1 - q2*q2 + q3*q3;
NaotoMorita 19:3fae66745363 313
NaotoMorita 19:3fae66745363 314 }
NaotoMorita 19:3fae66745363 315
NaotoMorita 19:3fae66745363 316 void ScErrStateEKF::defineQhat(Vector3 align){
NaotoMorita 19:3fae66745363 317 float cos_z_2 = cosf(0.5f*align.z);
NaotoMorita 19:3fae66745363 318 float cos_y_2 = cosf(0.5f*align.y);
NaotoMorita 19:3fae66745363 319 float cos_x_2 = cosf(0.5f*align.x);
NaotoMorita 19:3fae66745363 320
NaotoMorita 19:3fae66745363 321 float sin_z_2 = sinf(0.5f*align.z);
NaotoMorita 19:3fae66745363 322 float sin_y_2 = sinf(0.5f*align.y);
NaotoMorita 19:3fae66745363 323 float sin_x_2 = sinf(0.5f*align.x);
NaotoMorita 19:3fae66745363 324
NaotoMorita 19:3fae66745363 325 // and now compute quaternion
NaotoMorita 19:3fae66745363 326 qhat(1,1) = cos_z_2*cos_y_2*cos_x_2 + sin_z_2*sin_y_2*sin_x_2;
NaotoMorita 19:3fae66745363 327 qhat(2,1) = cos_z_2*cos_y_2*sin_x_2 - sin_z_2*sin_y_2*cos_x_2;
NaotoMorita 19:3fae66745363 328 qhat(3,1) = cos_z_2*sin_y_2*cos_x_2 + sin_z_2*cos_y_2*sin_x_2;
NaotoMorita 19:3fae66745363 329 qhat(4,1) = sin_z_2*cos_y_2*cos_x_2 - cos_z_2*sin_y_2*sin_x_2;
NaotoMorita 19:3fae66745363 330 }
NaotoMorita 19:3fae66745363 331
NaotoMorita 19:3fae66745363 332
NaotoMorita 19:3fae66745363 333
NaotoMorita 19:3fae66745363 334 Vector3 ScErrStateEKF::calcDynAcc(Vector3 acc, Vector3 accref)
NaotoMorita 19:3fae66745363 335 {
NaotoMorita 19:3fae66745363 336 Matrix dcm(3,3);
NaotoMorita 19:3fae66745363 337 computeDcm(dcm, qhat);
NaotoMorita 19:3fae66745363 338 float _x, _y, _z;
NaotoMorita 19:3fae66745363 339 _x = acc.x-(dcm( 1, 1 )*accref.x+dcm( 1, 2 )*accref.y+dcm( 1, 3 )*accref.z);
NaotoMorita 19:3fae66745363 340 _y = acc.y-(dcm( 2, 1 )*accref.x+dcm( 2, 2 )*accref.y+dcm( 2, 3 )*accref.z);
NaotoMorita 19:3fae66745363 341 _z = acc.z-(dcm( 3, 1 )*accref.x+dcm( 3, 2 )*accref.y+dcm( 3, 3 )*accref.z);
NaotoMorita 19:3fae66745363 342 return Vector3(_x, _y, _z);
NaotoMorita 19:3fae66745363 343 }
NaotoMorita 19:3fae66745363 344
NaotoMorita 19:3fae66745363 345
NaotoMorita 19:3fae66745363 346 bool ScErrStateEKF::determinDynStatus(Vector3 acc, Vector3 accref)
NaotoMorita 19:3fae66745363 347 {
NaotoMorita 19:3fae66745363 348 histffunc[histffuncindex] = acc.Norm()*acc.Norm()-accref.Norm()*accref.Norm()-3.0f*sigma2a;
NaotoMorita 19:3fae66745363 349 if(histffuncindex<9){
NaotoMorita 19:3fae66745363 350 histffuncindex += 1;
NaotoMorita 19:3fae66745363 351 }else{
NaotoMorita 19:3fae66745363 352 histffuncindex =0;
NaotoMorita 19:3fae66745363 353 }
NaotoMorita 19:3fae66745363 354 aveffunc = 0;
NaotoMorita 19:3fae66745363 355 for(int i = 1;i<10;i++){
NaotoMorita 19:3fae66745363 356 aveffunc += 1.0f/10.0f*histffunc[i];
NaotoMorita 19:3fae66745363 357 }
NaotoMorita 19:3fae66745363 358 sigma2f = 1.0f/10.0f*(6.0f*sigma2a*sigma2a+4.0f*accref.Norm()*accref.Norm()*sigma2a);
NaotoMorita 19:3fae66745363 359
NaotoMorita 19:3fae66745363 360
NaotoMorita 19:3fae66745363 361 dynacc = calcDynAcc(acc,accref);
NaotoMorita 19:3fae66745363 362 bool dynCase = true;
NaotoMorita 19:3fae66745363 363 if((dynacc.Norm()<0.005f) && (abs(acc.Norm()-accref.Norm())<0.005f)){dynCase = false;}
NaotoMorita 19:3fae66745363 364 if(aveffunc<sqrt(sigma2f/0.99f) && abs(acc.Norm()-accref.Norm())<0.001f){dynCase = false;}
NaotoMorita 19:3fae66745363 365 return dynCase;
NaotoMorita 19:3fae66745363 366
NaotoMorita 19:3fae66745363 367 }
NaotoMorita 19:3fae66745363 368
NaotoMorita 22:7d84b8bc20b4 369 /*
NaotoMorita 22:7d84b8bc20b4 370 void ScErrStateEKF::updateNominal(Vector3 gyro, Vector3 acc, Vector3 accref, float att_dt)
NaotoMorita 22:7d84b8bc20b4 371 {
NaotoMorita 22:7d84b8bc20b4 372 gyro -= gyroBias;
NaotoMorita 22:7d84b8bc20b4 373 acc -= accBias;
NaotoMorita 22:7d84b8bc20b4 374 Matrix A(4,4);
NaotoMorita 22:7d84b8bc20b4 375 A << 0.0f << -0.5f*gyro.x <<-0.5f*gyro.y <<-0.5f*gyro.z
NaotoMorita 22:7d84b8bc20b4 376 << 0.5f*gyro.x << 0.0f << 0.5f*gyro.z <<-0.5f*gyro.y
NaotoMorita 22:7d84b8bc20b4 377 << 0.5f*gyro.y << -0.5f*gyro.z << 0.0f << 0.5f*gyro.x
NaotoMorita 22:7d84b8bc20b4 378 << 0.5f*gyro.z << 0.5f*gyro.y <<-0.5f*gyro.x << 0.0f ;
NaotoMorita 22:7d84b8bc20b4 379
NaotoMorita 22:7d84b8bc20b4 380 Matrix phi = MatrixMath::Eye(4) + A * att_dt + (0.5f*att_dt*att_dt) * A * A;
NaotoMorita 22:7d84b8bc20b4 381 qhat = phi * qhat;
NaotoMorita 22:7d84b8bc20b4 382 float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qhat),qhat));
NaotoMorita 22:7d84b8bc20b4 383 qhat *= (1.0f/ qnorm);
NaotoMorita 22:7d84b8bc20b4 384
NaotoMorita 22:7d84b8bc20b4 385 Matrix dcm(3,3);
NaotoMorita 22:7d84b8bc20b4 386 computeDcm(dcm, qhat);
NaotoMorita 22:7d84b8bc20b4 387 vihat += (MatrixMath::Transpose(dcm)*MatrixMath::Vector2mat(acc)-MatrixMath::Vector2mat(accref))*att_dt*9.8f;
NaotoMorita 22:7d84b8bc20b4 388 }
NaotoMorita 19:3fae66745363 389
NaotoMorita 22:7d84b8bc20b4 390 void ScErrStateEKF::updateErrState(Vector3 gyro,Vector3 acc, float att_dt)
NaotoMorita 22:7d84b8bc20b4 391 {
NaotoMorita 22:7d84b8bc20b4 392 gyro -= gyroBias;
NaotoMorita 22:7d84b8bc20b4 393 acc -= accBias;
NaotoMorita 22:7d84b8bc20b4 394 Matrix A(nState,nState);
NaotoMorita 22:7d84b8bc20b4 395 A(1,2) = gyro.z;
NaotoMorita 22:7d84b8bc20b4 396 A(1,3) = -gyro.y;
NaotoMorita 22:7d84b8bc20b4 397 A(2,1) = -gyro.z;
NaotoMorita 22:7d84b8bc20b4 398 A(2,3) = gyro.x;
NaotoMorita 22:7d84b8bc20b4 399 A(3,1) = gyro.y;
NaotoMorita 22:7d84b8bc20b4 400 A(3,2) = -gyro.x;
NaotoMorita 22:7d84b8bc20b4 401 A(1,4) = -0.5f;
NaotoMorita 22:7d84b8bc20b4 402 A(2,5) = -0.5f;
NaotoMorita 22:7d84b8bc20b4 403 A(3,6) = -0.5f;
NaotoMorita 22:7d84b8bc20b4 404
NaotoMorita 22:7d84b8bc20b4 405 Matrix dcm(3,3);
NaotoMorita 22:7d84b8bc20b4 406 computeDcm(dcm, qhat);
NaotoMorita 22:7d84b8bc20b4 407 Matrix qeterm = -2.0f*9.8f*MatrixMath::Transpose(dcm)*MatrixMath::Matrixcross(acc.x,acc.y,acc.z);
NaotoMorita 22:7d84b8bc20b4 408 Matrix baterm = -9.8f*MatrixMath::Transpose(dcm);
NaotoMorita 22:7d84b8bc20b4 409 for (int i = 1; i < 4; i++){
NaotoMorita 22:7d84b8bc20b4 410 for (int j = 1; i < 4; i++){
NaotoMorita 22:7d84b8bc20b4 411 A(i+9,j) = qeterm(i,j);
NaotoMorita 22:7d84b8bc20b4 412 A(i+9,j+6) = baterm(i,j);
NaotoMorita 22:7d84b8bc20b4 413 }
NaotoMorita 22:7d84b8bc20b4 414 }
NaotoMorita 22:7d84b8bc20b4 415
NaotoMorita 22:7d84b8bc20b4 416 Matrix phi = MatrixMath::Eye(nState) + A * att_dt + (0.5f*att_dt*att_dt) * A * A;
NaotoMorita 22:7d84b8bc20b4 417 Matrix Qd = Q * att_dt + 0.5f*A*Q * att_dt * att_dt+ 0.5f*Q*MatrixMath::Transpose(A) * att_dt * att_dt;
NaotoMorita 22:7d84b8bc20b4 418 errState = phi * errState;
NaotoMorita 22:7d84b8bc20b4 419 Phat = phi*Phat*MatrixMath::Transpose(phi)+Qd;
NaotoMorita 22:7d84b8bc20b4 420 }
NaotoMorita 22:7d84b8bc20b4 421
NaotoMorita 22:7d84b8bc20b4 422 void ScErrStateEKF::updateVelocityConstraints()
NaotoMorita 22:7d84b8bc20b4 423 {
NaotoMorita 22:7d84b8bc20b4 424
NaotoMorita 22:7d84b8bc20b4 425 Matrix dcm(3,3);
NaotoMorita 22:7d84b8bc20b4 426 computeDcm(dcm, qhat);
NaotoMorita 22:7d84b8bc20b4 427 Matrix H(2,nState);
NaotoMorita 22:7d84b8bc20b4 428
NaotoMorita 22:7d84b8bc20b4 429 Matrix nomVb = dcm*vihat;
NaotoMorita 22:7d84b8bc20b4 430 Matrix qeterm = -2.0f*MatrixMath::Matrixcross(nomVb(1,1),nomVb(2,1),nomVb(3,1));
NaotoMorita 22:7d84b8bc20b4 431 for(int j = 1;j<4;j++){
NaotoMorita 22:7d84b8bc20b4 432 H(1,j) = -qeterm(1,j);
NaotoMorita 22:7d84b8bc20b4 433 H(2,j) = -qeterm(3,j);
NaotoMorita 22:7d84b8bc20b4 434 //H(3,j) = -qeterm(3,j);
NaotoMorita 22:7d84b8bc20b4 435 H(1,9+j) = -dcm(1,j);
NaotoMorita 22:7d84b8bc20b4 436 H(2,9+j) = -dcm(3,j);
NaotoMorita 22:7d84b8bc20b4 437 //H(3,9+j) = -dcm(3,j);
NaotoMorita 22:7d84b8bc20b4 438 }
NaotoMorita 22:7d84b8bc20b4 439
NaotoMorita 22:7d84b8bc20b4 440 Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+Rvsc);
NaotoMorita 22:7d84b8bc20b4 441 Matrix z(2,1);
NaotoMorita 22:7d84b8bc20b4 442 z << nomVb(1,1) << nomVb(3,1) ;
NaotoMorita 22:7d84b8bc20b4 443 Matrix corrVal = K * (z-H*errState);
NaotoMorita 22:7d84b8bc20b4 444 errState = errState + corrVal;
NaotoMorita 22:7d84b8bc20b4 445 Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*(Rvsc)*MatrixMath::Transpose(K);
NaotoMorita 22:7d84b8bc20b4 446
NaotoMorita 22:7d84b8bc20b4 447 }
NaotoMorita 22:7d84b8bc20b4 448
NaotoMorita 22:7d84b8bc20b4 449 void ScErrStateEKF::updateConstraints(Vector3 gyro)
NaotoMorita 22:7d84b8bc20b4 450 {
NaotoMorita 22:7d84b8bc20b4 451
NaotoMorita 22:7d84b8bc20b4 452 gyro -=gyroBias;
NaotoMorita 22:7d84b8bc20b4 453
NaotoMorita 22:7d84b8bc20b4 454 Matrix dcm(3,3);
NaotoMorita 22:7d84b8bc20b4 455 computeDcm(dcm, qhat);
NaotoMorita 22:7d84b8bc20b4 456
NaotoMorita 22:7d84b8bc20b4 457 Matrix qeterm = -2.0f*MatrixMath::Transpose(dcm)*MatrixMath::Matrixcross(gyro.x,gyro.y,gyro.z);
NaotoMorita 22:7d84b8bc20b4 458 Matrix H(4,nState);
NaotoMorita 22:7d84b8bc20b4 459 for(int i = 1; i<3; i++){
NaotoMorita 22:7d84b8bc20b4 460 for(int j = 1;j<4;j++){
NaotoMorita 22:7d84b8bc20b4 461 H(i,j) = qeterm(i,j);
NaotoMorita 22:7d84b8bc20b4 462 }
NaotoMorita 22:7d84b8bc20b4 463 }
NaotoMorita 22:7d84b8bc20b4 464 H(1,4) = 1.0f*(dcm(1,1));
NaotoMorita 22:7d84b8bc20b4 465 H(1,5) = 1.0f*(dcm(2,1));
NaotoMorita 22:7d84b8bc20b4 466 H(1,6) = 1.0f*(dcm(3,1));
NaotoMorita 22:7d84b8bc20b4 467 H(2,4) = 1.0f*(dcm(1,2));
NaotoMorita 22:7d84b8bc20b4 468 H(2,5) = 1.0f*(dcm(2,2));
NaotoMorita 22:7d84b8bc20b4 469 H(2,6) = 1.0f*(dcm(3,2));
NaotoMorita 22:7d84b8bc20b4 470
NaotoMorita 22:7d84b8bc20b4 471 Matrix nomVb = dcm*vihat;
NaotoMorita 22:7d84b8bc20b4 472 qeterm = -2.0f*MatrixMath::Matrixcross(nomVb(1,1),nomVb(2,1),nomVb(3,1));
NaotoMorita 22:7d84b8bc20b4 473 for(int j = 1;j<4;j++){
NaotoMorita 22:7d84b8bc20b4 474 H(3,j) = -qeterm(1,j);
NaotoMorita 22:7d84b8bc20b4 475 H(4,j) = -qeterm(3,j);
NaotoMorita 22:7d84b8bc20b4 476 //H(3,j) = -qeterm(3,j);
NaotoMorita 22:7d84b8bc20b4 477 H(3,9+j) = -dcm(1,j);
NaotoMorita 22:7d84b8bc20b4 478 H(4,9+j) = -dcm(3,j);
NaotoMorita 22:7d84b8bc20b4 479 //H(3,9+j) = -dcm(3,j);
NaotoMorita 22:7d84b8bc20b4 480 }
NaotoMorita 22:7d84b8bc20b4 481
NaotoMorita 22:7d84b8bc20b4 482 Matrix R(4,4);
NaotoMorita 22:7d84b8bc20b4 483 R(1,1) = Rgsc(1,1);
NaotoMorita 22:7d84b8bc20b4 484 R(2,2) = Rgsc(2,1);
NaotoMorita 22:7d84b8bc20b4 485 R(3,3) = Rvsc(1,1);
NaotoMorita 22:7d84b8bc20b4 486 R(4,4) = Rvsc(2,1);
NaotoMorita 22:7d84b8bc20b4 487 //R(5,5) = Rvsc(3,1);
NaotoMorita 22:7d84b8bc20b4 488 Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
NaotoMorita 22:7d84b8bc20b4 489 Matrix z(4,1);
NaotoMorita 22:7d84b8bc20b4 490 z <<dcm(1,1)*gyro.x+dcm(2,1)*gyro.y+dcm(3,1)*gyro.z<< dcm(1,2)*gyro.x+dcm(2,2)*gyro.y+dcm(3,2)*gyro.z << nomVb(1,1) << nomVb(3,1) ;
NaotoMorita 22:7d84b8bc20b4 491 Matrix corrVal = K * (z-H*errState);
NaotoMorita 22:7d84b8bc20b4 492 errState = errState + corrVal;
NaotoMorita 22:7d84b8bc20b4 493 Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*(R)*MatrixMath::Transpose(K);
NaotoMorita 22:7d84b8bc20b4 494 }
NaotoMorita 22:7d84b8bc20b4 495
NaotoMorita 22:7d84b8bc20b4 496 void ScErrStateEKF::computeVb(Vector3& vb)
NaotoMorita 22:7d84b8bc20b4 497 {
NaotoMorita 22:7d84b8bc20b4 498 Matrix dcm(3,3);
NaotoMorita 22:7d84b8bc20b4 499 computeDcm(dcm, qhat);
NaotoMorita 22:7d84b8bc20b4 500 Matrix vbmat = dcm*vihat;
NaotoMorita 22:7d84b8bc20b4 501 vb.x = vbmat(1,1);
NaotoMorita 22:7d84b8bc20b4 502 vb.y = vbmat(2,1);
NaotoMorita 22:7d84b8bc20b4 503 vb.z = vbmat(3,1);
NaotoMorita 22:7d84b8bc20b4 504
NaotoMorita 22:7d84b8bc20b4 505 }
NaotoMorita 22:7d84b8bc20b4 506 */
NaotoMorita 22:7d84b8bc20b4 507
NaotoMorita 22:7d84b8bc20b4 508