HAPSRG / solaESKF_Eigen

Eigen

Dependencies:   Eigen

Dependents:   optWingforHAPS_Eigen hexaTest_Eigen

ScErrStateEKF.cpp@21:d6079def0473, 2021-08-10 (annotated)

Committer:
NaotoMorita
Date:
Tue Aug 10 08:08:25 2021 +0000
Revision:
21:d6079def0473
Parent:
20:37d3c3ee36e9
Child:
22:7d84b8bc20b4
velocity canceled

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