HAPSRG / solaESKF_Eigen

Eigen

Dependencies:   Eigen

Dependents:   optWingforHAPS_Eigen hexaTest_Eigen

ScErrStateEKF.cpp@34:dec4b37db3f1, 2021-10-08 (annotated)

Committer:
NaotoMorita
Date:
Fri Oct 08 08:11:17 2021 +0000
Revision:
34:dec4b37db3f1
Parent:
32:321a756e12ad
Child:
38:1f6532849c05
for osaka

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 31:e655d4d8e4d6 11 :qhat(4,1),vihat(3,1), errState(9,1), bgState(3,1), Phat(9,9), Q(9,9),Phatbg(3,3),Qbg(3,3), Ra(3,3), Rgps(3,3),Rsr(1,1), Rm(3,3), Qab(3,3),Rgsc(2,2),Rvsc(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 34:dec4b37db3f1 16 setDiag(Phat,0.1f); //0.1
NaotoMorita 34:dec4b37db3f1 17 setDiag(Phatbg,0.0001f);
NaotoMorita 34:dec4b37db3f1 18 setQqerr(0.0000005f); //0.000005
NaotoMorita 32:321a756e12ad 19 setQgbias(0.001f);
NaotoMorita 31:e655d4d8e4d6 20 setQabias(0.00001f);
NaotoMorita 32:321a756e12ad 21 setQv(0.0001f);
NaotoMorita 19:3fae66745363 22
NaotoMorita 20:37d3c3ee36e9 23 //加速度の観測
NaotoMorita 23:1509648c2318 24 setDiag(Ra,0.005f);
NaotoMorita 32:321a756e12ad 25 setDiag(Qab,10.0f);
NaotoMorita 20:37d3c3ee36e9 26
NaotoMorita 20:37d3c3ee36e9 27 //ジャイロバイアスに関する制約
NaotoMorita 22:7d84b8bc20b4 28 setDiag(Rgsc,500.0f);
NaotoMorita 20:37d3c3ee36e9 29
NaotoMorita 34:dec4b37db3f1 30 //地磁気（未使用）
NaotoMorita 21:d6079def0473 31 setDiag(Rm,5.0f);
NaotoMorita 19:3fae66745363 32
NaotoMorita 34:dec4b37db3f1 33 //GPS
NaotoMorita 31:e655d4d8e4d6 34 setDiag(Rgps,5.5f);
NaotoMorita 34:dec4b37db3f1 35 //降下速度
NaotoMorita 31:e655d4d8e4d6 36 setDiag(Rsr,5.5f);
NaotoMorita 26:73c3f58b9d70 37
NaotoMorita 19:3fae66745363 38 for(int i = 0; i<10;i++){
NaotoMorita 19:3fae66745363 39 histffunc[i] = 0.0f;
NaotoMorita 19:3fae66745363 40 }
NaotoMorita 19:3fae66745363 41 histffuncindex = 0 ;
NaotoMorita 19:3fae66745363 42 sigma2a = 0.000020f;
NaotoMorita 19:3fae66745363 43
NaotoMorita 19:3fae66745363 44
NaotoMorita 19:3fae66745363 45 }
NaotoMorita 19:3fae66745363 46
NaotoMorita 23:1509648c2318 47 void ScErrStateEKF::updateNominal(Vector3 gyro, Vector3 acc, Vector3 accref, float att_dt)
NaotoMorita 19:3fae66745363 48 {
NaotoMorita 19:3fae66745363 49 gyro -= gyroBias;
NaotoMorita 23:1509648c2318 50 acc -= accBias;
NaotoMorita 19:3fae66745363 51 Matrix A(4,4);
NaotoMorita 19:3fae66745363 52 A << 0.0f << -0.5f*gyro.x <<-0.5f*gyro.y <<-0.5f*gyro.z
NaotoMorita 19:3fae66745363 53 << 0.5f*gyro.x << 0.0f << 0.5f*gyro.z <<-0.5f*gyro.y
NaotoMorita 19:3fae66745363 54 << 0.5f*gyro.y << -0.5f*gyro.z << 0.0f << 0.5f*gyro.x
NaotoMorita 19:3fae66745363 55 << 0.5f*gyro.z << 0.5f*gyro.y <<-0.5f*gyro.x << 0.0f ;
NaotoMorita 19:3fae66745363 56
NaotoMorita 20:37d3c3ee36e9 57 Matrix phi = MatrixMath::Eye(4) + A * att_dt + (0.5f*att_dt*att_dt) * A * A;
NaotoMorita 19:3fae66745363 58 qhat = phi * qhat;
NaotoMorita 19:3fae66745363 59 float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qhat),qhat));
NaotoMorita 19:3fae66745363 60 qhat *= (1.0f/ qnorm);
NaotoMorita 23:1509648c2318 61
NaotoMorita 23:1509648c2318 62 Matrix dcm(3,3);
NaotoMorita 23:1509648c2318 63 computeDcm(dcm, qhat);
NaotoMorita 23:1509648c2318 64 vihat += (MatrixMath::Transpose(dcm)*MatrixMath::Vector2mat(acc)-MatrixMath::Vector2mat(accref))*att_dt*9.8f;
NaotoMorita 19:3fae66745363 65 }
NaotoMorita 19:3fae66745363 66
NaotoMorita 23:1509648c2318 67 void ScErrStateEKF::updateErrState(Vector3 gyro,Vector3 acc, float att_dt)
NaotoMorita 23:1509648c2318 68 {
NaotoMorita 23:1509648c2318 69 gyro -= gyroBias;
NaotoMorita 23:1509648c2318 70 acc -= accBias;
NaotoMorita 23:1509648c2318 71 Matrix A(nState,nState);
NaotoMorita 23:1509648c2318 72 A(1,2) = gyro.z;
NaotoMorita 23:1509648c2318 73 A(1,3) = -gyro.y;
NaotoMorita 23:1509648c2318 74 A(2,1) = -gyro.z;
NaotoMorita 23:1509648c2318 75 A(2,3) = gyro.x;
NaotoMorita 23:1509648c2318 76 A(3,1) = gyro.y;
NaotoMorita 23:1509648c2318 77 A(3,2) = -gyro.x;
NaotoMorita 23:1509648c2318 78
NaotoMorita 23:1509648c2318 79 Matrix dcm(3,3);
NaotoMorita 23:1509648c2318 80 computeDcm(dcm, qhat);
NaotoMorita 23:1509648c2318 81 Matrix qeterm = -2.0f*9.8f*MatrixMath::Transpose(dcm)*MatrixMath::Matrixcross(acc.x,acc.y,acc.z);
NaotoMorita 23:1509648c2318 82 Matrix baterm = -9.8f*MatrixMath::Transpose(dcm);
NaotoMorita 23:1509648c2318 83 for (int i = 1; i < 4; i++){
NaotoMorita 23:1509648c2318 84 for (int j = 1; i < 4; i++){
NaotoMorita 31:e655d4d8e4d6 85 A(i+6,j) = qeterm(i,j);
NaotoMorita 31:e655d4d8e4d6 86 A(i+6,j+3) = baterm(i,j);
NaotoMorita 23:1509648c2318 87 }
NaotoMorita 23:1509648c2318 88 }
NaotoMorita 23:1509648c2318 89
NaotoMorita 23:1509648c2318 90 Matrix phi = MatrixMath::Eye(nState) + A * att_dt + (0.5f*att_dt*att_dt) * A * A;
NaotoMorita 23:1509648c2318 91 Matrix Qd = Q * att_dt + 0.5f*A*Q * att_dt * att_dt+ 0.5f*Q*MatrixMath::Transpose(A) * att_dt * att_dt;
NaotoMorita 23:1509648c2318 92 errState = phi * errState;
NaotoMorita 23:1509648c2318 93 Phat = phi*Phat*MatrixMath::Transpose(phi)+Qd;
NaotoMorita 23:1509648c2318 94 }
NaotoMorita 20:37d3c3ee36e9 95 void ScErrStateEKF::setQqerr(float val){
NaotoMorita 20:37d3c3ee36e9 96 Q(1,1) = val;
NaotoMorita 20:37d3c3ee36e9 97 Q(2,2) = val;
NaotoMorita 20:37d3c3ee36e9 98 Q(3,3) = val;
NaotoMorita 20:37d3c3ee36e9 99 }
NaotoMorita 20:37d3c3ee36e9 100
NaotoMorita 20:37d3c3ee36e9 101 void ScErrStateEKF::setQgbias(float val){
NaotoMorita 31:e655d4d8e4d6 102 Qbg(1,1) = val;
NaotoMorita 31:e655d4d8e4d6 103 Qbg(2,2) = val;
NaotoMorita 31:e655d4d8e4d6 104 Qbg(2,2) = val;
NaotoMorita 31:e655d4d8e4d6 105 }
NaotoMorita 31:e655d4d8e4d6 106 void ScErrStateEKF::setQabias(float val){
NaotoMorita 20:37d3c3ee36e9 107 Q(4,4) = val;
NaotoMorita 20:37d3c3ee36e9 108 Q(5,5) = val;
NaotoMorita 20:37d3c3ee36e9 109 Q(6,6) = val;
NaotoMorita 20:37d3c3ee36e9 110 }
NaotoMorita 31:e655d4d8e4d6 111 void ScErrStateEKF::setQv(float val){
NaotoMorita 20:37d3c3ee36e9 112 Q(7,7) = val;
NaotoMorita 20:37d3c3ee36e9 113 Q(8,8) = val;
NaotoMorita 20:37d3c3ee36e9 114 Q(9,9) = val;
NaotoMorita 20:37d3c3ee36e9 115 }
NaotoMorita 21:d6079def0473 116
NaotoMorita 20:37d3c3ee36e9 117 void ScErrStateEKF::setQab(float val){
NaotoMorita 21:d6079def0473 118 setDiag(Qab,val);
NaotoMorita 20:37d3c3ee36e9 119 }
NaotoMorita 20:37d3c3ee36e9 120
NaotoMorita 23:1509648c2318 121
NaotoMorita 20:37d3c3ee36e9 122 void ScErrStateEKF::setRsoftconst(float Vgsc,float Vvsc){
NaotoMorita 21:d6079def0473 123 setDiag(Rgsc,Vgsc);
NaotoMorita 23:1509648c2318 124 setDiag(Rvsc,Vvsc);
NaotoMorita 20:37d3c3ee36e9 125 }
NaotoMorita 20:37d3c3ee36e9 126
NaotoMorita 21:d6079def0473 127 void ScErrStateEKF::setDiag(Matrix& mat, float val){
NaotoMorita 21:d6079def0473 128 for (int i = 1; i < mat.getCols()+1; i++){
NaotoMorita 21:d6079def0473 129 mat(i,i) = val;
NaotoMorita 21:d6079def0473 130 }
NaotoMorita 21:d6079def0473 131 }
NaotoMorita 20:37d3c3ee36e9 132
NaotoMorita 25:07ac5c6cd61c 133 void ScErrStateEKF::getQhat(float (&res)[4]){
NaotoMorita 25:07ac5c6cd61c 134 for (int i = 0; i < 4; i++){
NaotoMorita 25:07ac5c6cd61c 135 res[i] = qhat(i+1,1);
NaotoMorita 25:07ac5c6cd61c 136 }
NaotoMorita 25:07ac5c6cd61c 137 }
NaotoMorita 25:07ac5c6cd61c 138
NaotoMorita 25:07ac5c6cd61c 139 void ScErrStateEKF::getVihat(float (&res)[3]){
NaotoMorita 25:07ac5c6cd61c 140 for (int i = 0; i < 3; i++){
NaotoMorita 25:07ac5c6cd61c 141 res[i] = vihat(i+1,1);
NaotoMorita 25:07ac5c6cd61c 142 }
NaotoMorita 25:07ac5c6cd61c 143 }
NaotoMorita 25:07ac5c6cd61c 144
NaotoMorita 25:07ac5c6cd61c 145 void ScErrStateEKF::getGyroBias(float (&resVal)[3], float (&resCov)[6]){
NaotoMorita 25:07ac5c6cd61c 146 resVal[0] = gyroBias.x;
NaotoMorita 25:07ac5c6cd61c 147 resVal[1] = gyroBias.y;
NaotoMorita 25:07ac5c6cd61c 148 resVal[2] = gyroBias.z;
NaotoMorita 31:e655d4d8e4d6 149 resCov[0] = Phatbg(1,1);
NaotoMorita 31:e655d4d8e4d6 150 resCov[1] = Phatbg(1,2);
NaotoMorita 31:e655d4d8e4d6 151 resCov[2] = Phatbg(1,3);
NaotoMorita 31:e655d4d8e4d6 152 resCov[3] = Phatbg(2,2);
NaotoMorita 31:e655d4d8e4d6 153 resCov[4] = Phatbg(2,3);
NaotoMorita 31:e655d4d8e4d6 154 resCov[5] = Phatbg(3,3);
NaotoMorita 25:07ac5c6cd61c 155
NaotoMorita 25:07ac5c6cd61c 156 }
NaotoMorita 25:07ac5c6cd61c 157
NaotoMorita 25:07ac5c6cd61c 158 void ScErrStateEKF::getAccBias(float (&resVal)[3], float (&resCov)[6]){
NaotoMorita 25:07ac5c6cd61c 159 resVal[0] = accBias.x;
NaotoMorita 25:07ac5c6cd61c 160 resVal[1] = accBias.y;
NaotoMorita 25:07ac5c6cd61c 161 resVal[2] = accBias.z;
NaotoMorita 31:e655d4d8e4d6 162 resCov[0] = Phat(4,4);
NaotoMorita 31:e655d4d8e4d6 163 resCov[1] = Phat(4,5);
NaotoMorita 31:e655d4d8e4d6 164 resCov[2] = Phat(4,6);
NaotoMorita 31:e655d4d8e4d6 165 resCov[3] = Phat(5,5);
NaotoMorita 31:e655d4d8e4d6 166 resCov[4] = Phat(5,6);
NaotoMorita 31:e655d4d8e4d6 167 resCov[5] = Phat(6,6);
NaotoMorita 25:07ac5c6cd61c 168
NaotoMorita 25:07ac5c6cd61c 169 }
NaotoMorita 25:07ac5c6cd61c 170
NaotoMorita 19:3fae66745363 171
NaotoMorita 19:3fae66745363 172 void ScErrStateEKF::updateAccMeasures(Vector3 acc, Vector3 accref)
NaotoMorita 19:3fae66745363 173 {
NaotoMorita 21:d6079def0473 174 acc -= accBias;
NaotoMorita 20:37d3c3ee36e9 175 Matrix dcm(3,3);
NaotoMorita 20:37d3c3ee36e9 176 computeDcm(dcm, qhat);
NaotoMorita 20:37d3c3ee36e9 177 Vector3 gvec(dcm(1,3)*accref.z, dcm(2,3)*accref.z, dcm(3,3)*accref.z);
NaotoMorita 21:d6079def0473 178 Matrix H(3,nState);
NaotoMorita 20:37d3c3ee36e9 179 H(1,2) = -2.0f*gvec.z;
NaotoMorita 20:37d3c3ee36e9 180 H(1,3) = 2.0f*gvec.y;
NaotoMorita 20:37d3c3ee36e9 181 H(2,1) = 2.0f*gvec.z;
NaotoMorita 20:37d3c3ee36e9 182 H(2,3) = -2.0f*gvec.x;
NaotoMorita 20:37d3c3ee36e9 183 H(3,1) = -2.0f*gvec.y;
NaotoMorita 20:37d3c3ee36e9 184 H(3,2) = 2.0f*gvec.x;
NaotoMorita 31:e655d4d8e4d6 185 H(1,4) = 1.0f;
NaotoMorita 31:e655d4d8e4d6 186 H(2,5) = 1.0f;
NaotoMorita 31:e655d4d8e4d6 187 H(3,6) = 1.0f;
NaotoMorita 20:37d3c3ee36e9 188 Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+Ra+Qab);
NaotoMorita 21:d6079def0473 189 Matrix z = MatrixMath::Vector2mat(acc)-dcm*MatrixMath::Vector2mat(accref);
NaotoMorita 20:37d3c3ee36e9 190 Matrix corrVal = K * (z-H*errState);
NaotoMorita 20:37d3c3ee36e9 191 errState = errState + corrVal;
NaotoMorita 21:d6079def0473 192 Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*(Ra+Qab)*MatrixMath::Transpose(K);
NaotoMorita 20:37d3c3ee36e9 193 }
NaotoMorita 20:37d3c3ee36e9 194
NaotoMorita 20:37d3c3ee36e9 195 void ScErrStateEKF::updateGyroBiasConstraints(Vector3 gyro)
NaotoMorita 20:37d3c3ee36e9 196 {
NaotoMorita 21:d6079def0473 197 gyro -= gyroBias;
NaotoMorita 23:1509648c2318 198 Matrix dcm(3,3);
NaotoMorita 23:1509648c2318 199 computeDcm(dcm, qhat);
NaotoMorita 31:e655d4d8e4d6 200 Matrix H(2,3);
NaotoMorita 31:e655d4d8e4d6 201 H(1,1) = 1.0f*(dcm(1,1));
NaotoMorita 31:e655d4d8e4d6 202 H(1,2) = 1.0f*(dcm(2,1));
NaotoMorita 31:e655d4d8e4d6 203 H(1,3) = 1.0f*(dcm(3,1));
NaotoMorita 31:e655d4d8e4d6 204 H(2,1) = 1.0f*(dcm(1,2));
NaotoMorita 31:e655d4d8e4d6 205 H(2,2) = 1.0f*(dcm(2,2));
NaotoMorita 31:e655d4d8e4d6 206 H(2,3) = 1.0f*(dcm(3,2));
NaotoMorita 23:1509648c2318 207
NaotoMorita 31:e655d4d8e4d6 208 Matrix K = (Phatbg*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phatbg*MatrixMath::Transpose(H)+Rgsc);
NaotoMorita 31:e655d4d8e4d6 209 Matrix z(2,1);
NaotoMorita 31:e655d4d8e4d6 210 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 31:e655d4d8e4d6 211 Matrix corrVal = K * (z-H*bgState);
NaotoMorita 31:e655d4d8e4d6 212 bgState = bgState + corrVal;
NaotoMorita 31:e655d4d8e4d6 213 Phatbg = (MatrixMath::Eye(3)-K*H)*Phatbg*MatrixMath::Transpose(MatrixMath::Eye(3)-K*H)+K*(Rgsc)*MatrixMath::Transpose(K);
NaotoMorita 23:1509648c2318 214 }
NaotoMorita 23:1509648c2318 215
NaotoMorita 23:1509648c2318 216
NaotoMorita 19:3fae66745363 217 void ScErrStateEKF::updateStaticAccMeasures(Vector3 acc, Vector3 accref)
NaotoMorita 19:3fae66745363 218 {
NaotoMorita 19:3fae66745363 219 Matrix dcm(3,3);
NaotoMorita 19:3fae66745363 220 computeDcm(dcm, qhat);
NaotoMorita 19:3fae66745363 221 Vector3 gvec(dcm(1,3)*accref.z, dcm(2,3)*accref.z, dcm(3,3)*accref.z);
NaotoMorita 21:d6079def0473 222 Matrix H(3,nState);
NaotoMorita 19:3fae66745363 223 H(1,2) = -2.0f*gvec.z;
NaotoMorita 19:3fae66745363 224 H(1,3) = 2.0f*gvec.y;
NaotoMorita 19:3fae66745363 225 H(2,1) = 2.0f*gvec.z;
NaotoMorita 19:3fae66745363 226 H(2,3) = -2.0f*gvec.x;
NaotoMorita 19:3fae66745363 227 H(3,1) = -2.0f*gvec.y;
NaotoMorita 19:3fae66745363 228 H(3,2) = 2.0f*gvec.x;
NaotoMorita 19:3fae66745363 229 Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+Ra*100.0f);
NaotoMorita 19:3fae66745363 230 Matrix accmat(3,1);
NaotoMorita 19:3fae66745363 231 accmat << acc.x << acc.y << acc.z;
NaotoMorita 19:3fae66745363 232 Matrix gref(3,1);
NaotoMorita 19:3fae66745363 233 gref << 0.0f << 0.0f << accref.z;
NaotoMorita 19:3fae66745363 234 Matrix z = accmat-dcm*gref;
NaotoMorita 19:3fae66745363 235 errState = errState + K * (z-H*errState);
NaotoMorita 21:d6079def0473 236 Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*(Ra*100.0f)*MatrixMath::Transpose(K);
NaotoMorita 19:3fae66745363 237 }
NaotoMorita 19:3fae66745363 238
NaotoMorita 32:321a756e12ad 239 void ScErrStateEKF::updateGPSVelocity(float vi_x, float vi_y, float sinkRate,Vector3 acc, Vector3 accref)
NaotoMorita 19:3fae66745363 240 {
NaotoMorita 31:e655d4d8e4d6 241 acc -= accBias;
NaotoMorita 31:e655d4d8e4d6 242 Matrix dcm(3,3);
NaotoMorita 31:e655d4d8e4d6 243 computeDcm(dcm, qhat);
NaotoMorita 31:e655d4d8e4d6 244 Vector3 gvec(dcm(1,3)*accref.z, dcm(2,3)*accref.z, dcm(3,3)*accref.z);
NaotoMorita 32:321a756e12ad 245 Matrix H(6,nState);
NaotoMorita 31:e655d4d8e4d6 246 H(1,2) = -2.0f*gvec.z;
NaotoMorita 31:e655d4d8e4d6 247 H(1,3) = 2.0f*gvec.y;
NaotoMorita 31:e655d4d8e4d6 248 H(2,1) = 2.0f*gvec.z;
NaotoMorita 31:e655d4d8e4d6 249 H(2,3) = -2.0f*gvec.x;
NaotoMorita 31:e655d4d8e4d6 250 H(3,1) = -2.0f*gvec.y;
NaotoMorita 31:e655d4d8e4d6 251 H(3,2) = 2.0f*gvec.x;
NaotoMorita 31:e655d4d8e4d6 252 H(1,4) = 1.0f;
NaotoMorita 31:e655d4d8e4d6 253 H(2,5) = 1.0f;
NaotoMorita 31:e655d4d8e4d6 254 H(3,6) = 1.0f;
NaotoMorita 31:e655d4d8e4d6 255 H(4,7) = 1.0f;
NaotoMorita 31:e655d4d8e4d6 256 H(5,8) = 1.0f;
NaotoMorita 32:321a756e12ad 257 H(6,9) = 1.0f;
NaotoMorita 31:e655d4d8e4d6 258
NaotoMorita 32:321a756e12ad 259 Matrix R(6,6);
NaotoMorita 31:e655d4d8e4d6 260 R(1,1) = Ra(1,1)+Qab(1,1);
NaotoMorita 31:e655d4d8e4d6 261 R(2,2) = Ra(2,2)+Qab(2,2);
NaotoMorita 31:e655d4d8e4d6 262 R(3,3) = Ra(3,3)+Qab(3,3);
NaotoMorita 31:e655d4d8e4d6 263 R(4,4) = Rgps(1,1);
NaotoMorita 31:e655d4d8e4d6 264 R(5,5) = Rgps(2,2);
NaotoMorita 32:321a756e12ad 265 R(6,6) = Rsr(1,1);
NaotoMorita 31:e655d4d8e4d6 266
NaotoMorita 31:e655d4d8e4d6 267 Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
NaotoMorita 31:e655d4d8e4d6 268 Matrix zacc = MatrixMath::Vector2mat(acc)-dcm*MatrixMath::Vector2mat(accref);
NaotoMorita 32:321a756e12ad 269 Matrix z(6,1);
NaotoMorita 32:321a756e12ad 270 z << zacc(1,1)<< zacc(2,1)<< zacc(3,1) << vi_x - vihat(1,1) << vi_y-vihat(2,1) << sinkRate - vihat(3,1);;
NaotoMorita 26:73c3f58b9d70 271 Matrix corrVal = K * (z-H*errState);
NaotoMorita 26:73c3f58b9d70 272 errState = errState + corrVal;
NaotoMorita 31:e655d4d8e4d6 273 Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*(R)*MatrixMath::Transpose(K);
NaotoMorita 19:3fae66745363 274 }
NaotoMorita 19:3fae66745363 275
NaotoMorita 31:e655d4d8e4d6 276 void ScErrStateEKF::updateSinkRate(float sinkRate,Vector3 acc, Vector3 accref)
NaotoMorita 30:ff884e9b2e30 277 {
NaotoMorita 31:e655d4d8e4d6 278 acc -= accBias;
NaotoMorita 31:e655d4d8e4d6 279 Matrix dcm(3,3);
NaotoMorita 31:e655d4d8e4d6 280 computeDcm(dcm, qhat);
NaotoMorita 31:e655d4d8e4d6 281 Vector3 gvec(dcm(1,3)*accref.z, dcm(2,3)*accref.z, dcm(3,3)*accref.z);
NaotoMorita 31:e655d4d8e4d6 282 Matrix H(4,nState);
NaotoMorita 31:e655d4d8e4d6 283 H(1,2) = -2.0f*gvec.z;
NaotoMorita 31:e655d4d8e4d6 284 H(1,3) = 2.0f*gvec.y;
NaotoMorita 31:e655d4d8e4d6 285 H(2,1) = 2.0f*gvec.z;
NaotoMorita 31:e655d4d8e4d6 286 H(2,3) = -2.0f*gvec.x;
NaotoMorita 31:e655d4d8e4d6 287 H(3,1) = -2.0f*gvec.y;
NaotoMorita 31:e655d4d8e4d6 288 H(3,2) = 2.0f*gvec.x;
NaotoMorita 31:e655d4d8e4d6 289 H(1,4) = 1.0f;
NaotoMorita 31:e655d4d8e4d6 290 H(2,5) = 1.0f;
NaotoMorita 31:e655d4d8e4d6 291 H(3,6) = 1.0f;
NaotoMorita 31:e655d4d8e4d6 292 H(4,9) = 1.0f;
NaotoMorita 31:e655d4d8e4d6 293
NaotoMorita 31:e655d4d8e4d6 294 Matrix R(4,4);
NaotoMorita 31:e655d4d8e4d6 295 R(1,1) = Ra(1,1)+Qab(1,1);
NaotoMorita 31:e655d4d8e4d6 296 R(2,2) = Ra(2,2)+Qab(2,2);
NaotoMorita 31:e655d4d8e4d6 297 R(3,3) = Ra(3,3)+Qab(3,3);
NaotoMorita 31:e655d4d8e4d6 298 R(4,4) = Rsr(1,1);
NaotoMorita 31:e655d4d8e4d6 299
NaotoMorita 31:e655d4d8e4d6 300 Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
NaotoMorita 31:e655d4d8e4d6 301 Matrix zacc = MatrixMath::Vector2mat(acc)-dcm*MatrixMath::Vector2mat(accref);
NaotoMorita 31:e655d4d8e4d6 302 Matrix z(4,1);
NaotoMorita 31:e655d4d8e4d6 303 z << zacc(1,1)<< zacc(2,1)<< zacc(3,1) << sinkRate - vihat(3,1);
NaotoMorita 30:ff884e9b2e30 304 Matrix corrVal = K * (z-H*errState);
NaotoMorita 30:ff884e9b2e30 305 errState = errState + corrVal;
NaotoMorita 31:e655d4d8e4d6 306 Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*(R)*MatrixMath::Transpose(K);
NaotoMorita 30:ff884e9b2e30 307 }
NaotoMorita 30:ff884e9b2e30 308
NaotoMorita 25:07ac5c6cd61c 309 void ScErrStateEKF::updateCenteredGyroBiasCorrection(float (&cBg)[3])
NaotoMorita 25:07ac5c6cd61c 310 {
NaotoMorita 25:07ac5c6cd61c 311
NaotoMorita 25:07ac5c6cd61c 312 Matrix RgyroBias(3,3);
NaotoMorita 25:07ac5c6cd61c 313 for(int i = 1;i<4;i++){
NaotoMorita 25:07ac5c6cd61c 314 RgyroBias(i,i) = 0.1f;
NaotoMorita 25:07ac5c6cd61c 315 }
NaotoMorita 25:07ac5c6cd61c 316
NaotoMorita 31:e655d4d8e4d6 317 Matrix H(3,3);
NaotoMorita 31:e655d4d8e4d6 318 H(1,1) = 1.0f;
NaotoMorita 31:e655d4d8e4d6 319 H(2,2) = 1.0f;
NaotoMorita 31:e655d4d8e4d6 320 H(3,3) = 1.0f;
NaotoMorita 25:07ac5c6cd61c 321
NaotoMorita 31:e655d4d8e4d6 322 Matrix K = (Phatbg*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phatbg*MatrixMath::Transpose(H)+RgyroBias);
NaotoMorita 25:07ac5c6cd61c 323 Matrix z(3,1);
NaotoMorita 25:07ac5c6cd61c 324 z <<cBg[0]-gyroBias.x <<cBg[1]-gyroBias.y <<cBg[2]-gyroBias.z;
NaotoMorita 31:e655d4d8e4d6 325 Matrix corrVal = K * (z-H*bgState);
NaotoMorita 31:e655d4d8e4d6 326 bgState = bgState + corrVal;
NaotoMorita 31:e655d4d8e4d6 327 Phatbg = (MatrixMath::Eye(3)-K*H)*Phatbg*MatrixMath::Transpose(MatrixMath::Eye(3)-K*H)+K*(RgyroBias)*MatrixMath::Transpose(K);
NaotoMorita 25:07ac5c6cd61c 328
NaotoMorita 25:07ac5c6cd61c 329 }
NaotoMorita 25:07ac5c6cd61c 330
NaotoMorita 19:3fae66745363 331 void ScErrStateEKF::resetBias()
NaotoMorita 19:3fae66745363 332 {
NaotoMorita 31:e655d4d8e4d6 333 gyroBias.x = gyroBias.x + bgState(1,1)*1.0f;
NaotoMorita 31:e655d4d8e4d6 334 gyroBias.y = gyroBias.y + bgState(2,1)*1.0f;
NaotoMorita 31:e655d4d8e4d6 335 gyroBias.z = gyroBias.z + bgState(3,1)*1.0f;
NaotoMorita 31:e655d4d8e4d6 336 bgState(1,1) = 0.0f;
NaotoMorita 31:e655d4d8e4d6 337 bgState(2,1) = 0.0f;
NaotoMorita 31:e655d4d8e4d6 338 bgState(3,1) = 0.0f;
NaotoMorita 31:e655d4d8e4d6 339 accBias.x = accBias.x + errState(4,1);
NaotoMorita 31:e655d4d8e4d6 340 accBias.y = accBias.y + errState(5,1);
NaotoMorita 31:e655d4d8e4d6 341 accBias.z = accBias.z + errState(6,1);
NaotoMorita 19:3fae66745363 342 errState(4,1) = 0.0f;
NaotoMorita 19:3fae66745363 343 errState(5,1) = 0.0f;
NaotoMorita 19:3fae66745363 344 errState(6,1) = 0.0f;
NaotoMorita 19:3fae66745363 345 }
NaotoMorita 19:3fae66745363 346
NaotoMorita 19:3fae66745363 347 void ScErrStateEKF::computeAngles(Vector3& rpy,Vector3 rpy_align)
NaotoMorita 19:3fae66745363 348 {
NaotoMorita 19:3fae66745363 349 Matrix qerr(4,1);
NaotoMorita 19:3fae66745363 350 qerr << 1.0f << errState(1,1) << errState(2,1) << errState(3,1);
NaotoMorita 19:3fae66745363 351 //float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qerr),qerr));
NaotoMorita 19:3fae66745363 352 //qerr *= (1.0f/ qnorm);
NaotoMorita 19:3fae66745363 353 Matrix qest = quatmultiply(qhat, qerr);
NaotoMorita 19:3fae66745363 354 float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qest),qest));
NaotoMorita 19:3fae66745363 355 qest *= (1.0f/ qnorm);
NaotoMorita 19:3fae66745363 356 float q0 = qest( 1, 1 );
NaotoMorita 19:3fae66745363 357 float q1 = qest( 2, 1 );
NaotoMorita 19:3fae66745363 358 float q2 = qest( 3, 1 );
NaotoMorita 19:3fae66745363 359 float q3 = qest( 4, 1 );
NaotoMorita 19:3fae66745363 360 rpy.x = atan2f(q0*q1 + q2*q3, 0.5f - q1*q1 - q2*q2)-rpy_align.x;
NaotoMorita 19:3fae66745363 361 rpy.y = asinf(-2.0f * (q1*q3 - q0*q2))-rpy_align.y;
NaotoMorita 19:3fae66745363 362 rpy.z = atan2f(q1*q2 + q0*q3, 0.5f - q2*q2 - q3*q3);
NaotoMorita 19:3fae66745363 363
NaotoMorita 19:3fae66745363 364 }
NaotoMorita 19:3fae66745363 365
NaotoMorita 21:d6079def0473 366
NaotoMorita 21:d6079def0473 367 void ScErrStateEKF::fuseErr2Nominal()
NaotoMorita 19:3fae66745363 368 {
NaotoMorita 19:3fae66745363 369 Matrix qerr(4,1);
NaotoMorita 19:3fae66745363 370 qerr << 1.0f << errState(1,1) << errState(2,1) << errState(3,1);
NaotoMorita 19:3fae66745363 371 //float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qerr),qerr));
NaotoMorita 19:3fae66745363 372 //qerr *= (1.0f/ qnorm);
NaotoMorita 19:3fae66745363 373 qhat = quatmultiply(qhat, qerr);
NaotoMorita 19:3fae66745363 374 errState(1,1) = 0.0f;
NaotoMorita 19:3fae66745363 375 errState(2,1) = 0.0f;
NaotoMorita 19:3fae66745363 376 errState(3,1) = 0.0f;
NaotoMorita 19:3fae66745363 377 float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qhat),qhat));
NaotoMorita 19:3fae66745363 378 qhat *= (1.0f/ qnorm);
NaotoMorita 23:1509648c2318 379
NaotoMorita 31:e655d4d8e4d6 380 vihat(1,1) += errState(7,1);
NaotoMorita 31:e655d4d8e4d6 381 vihat(2,1) += errState(8,1);
NaotoMorita 31:e655d4d8e4d6 382 vihat(3,1) += errState(9,1);
NaotoMorita 31:e655d4d8e4d6 383 errState(7,1) = 0.0f;
NaotoMorita 31:e655d4d8e4d6 384 errState(8,1) = 0.0f;
NaotoMorita 31:e655d4d8e4d6 385 errState(9,1) = 0.0f;
NaotoMorita 19:3fae66745363 386 }
NaotoMorita 19:3fae66745363 387
NaotoMorita 19:3fae66745363 388 Matrix ScErrStateEKF::quatmultiply(Matrix q, Matrix r)
NaotoMorita 19:3fae66745363 389 {
NaotoMorita 19:3fae66745363 390 Matrix qout(4,1);
NaotoMorita 19:3fae66745363 391 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 392 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 393 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 394 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 395 float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qout),qout));
NaotoMorita 19:3fae66745363 396 qout *= (1.0f/ qnorm);
NaotoMorita 19:3fae66745363 397 return qout;
NaotoMorita 19:3fae66745363 398 }
NaotoMorita 19:3fae66745363 399
NaotoMorita 19:3fae66745363 400 void ScErrStateEKF::computeDcm(Matrix& dcm, Matrix quat)
NaotoMorita 19:3fae66745363 401 {
NaotoMorita 19:3fae66745363 402
NaotoMorita 19:3fae66745363 403 float q0 = quat( 1, 1 );
NaotoMorita 19:3fae66745363 404 float q1 = quat( 2, 1 );
NaotoMorita 19:3fae66745363 405 float q2 = quat( 3, 1 );
NaotoMorita 19:3fae66745363 406 float q3 = quat( 4, 1 );
NaotoMorita 19:3fae66745363 407
NaotoMorita 19:3fae66745363 408 dcm(1,1) = q0*q0 + q1*q1 - q2*q2 - q3*q3;
NaotoMorita 19:3fae66745363 409 dcm(1,2) = 2*(q1*q2 + q0*q3);
NaotoMorita 19:3fae66745363 410 dcm(1,3) = 2*(q1*q3 - q0*q2);
NaotoMorita 19:3fae66745363 411 dcm(2,1) = 2*(q1*q2 - q0*q3);
NaotoMorita 19:3fae66745363 412 dcm(2,2) = q0*q0 - q1*q1 + q2*q2 - q3*q3;
NaotoMorita 19:3fae66745363 413 dcm(2,3) = 2*(q2*q3 + q0*q1);
NaotoMorita 19:3fae66745363 414 dcm(3,1) = 2*(q1*q3 + q0*q2);
NaotoMorita 19:3fae66745363 415 dcm(3,2) = 2*(q2*q3 - q0*q1);
NaotoMorita 19:3fae66745363 416 dcm(3,3) = q0*q0 - q1*q1 - q2*q2 + q3*q3;
NaotoMorita 19:3fae66745363 417
NaotoMorita 19:3fae66745363 418 }
NaotoMorita 19:3fae66745363 419
NaotoMorita 19:3fae66745363 420 void ScErrStateEKF::defineQhat(Vector3 align){
NaotoMorita 19:3fae66745363 421 float cos_z_2 = cosf(0.5f*align.z);
NaotoMorita 19:3fae66745363 422 float cos_y_2 = cosf(0.5f*align.y);
NaotoMorita 19:3fae66745363 423 float cos_x_2 = cosf(0.5f*align.x);
NaotoMorita 19:3fae66745363 424
NaotoMorita 19:3fae66745363 425 float sin_z_2 = sinf(0.5f*align.z);
NaotoMorita 19:3fae66745363 426 float sin_y_2 = sinf(0.5f*align.y);
NaotoMorita 19:3fae66745363 427 float sin_x_2 = sinf(0.5f*align.x);
NaotoMorita 19:3fae66745363 428
NaotoMorita 19:3fae66745363 429 // and now compute quaternion
NaotoMorita 19:3fae66745363 430 qhat(1,1) = cos_z_2*cos_y_2*cos_x_2 + sin_z_2*sin_y_2*sin_x_2;
NaotoMorita 19:3fae66745363 431 qhat(2,1) = cos_z_2*cos_y_2*sin_x_2 - sin_z_2*sin_y_2*cos_x_2;
NaotoMorita 19:3fae66745363 432 qhat(3,1) = cos_z_2*sin_y_2*cos_x_2 + sin_z_2*cos_y_2*sin_x_2;
NaotoMorita 19:3fae66745363 433 qhat(4,1) = sin_z_2*cos_y_2*cos_x_2 - cos_z_2*sin_y_2*sin_x_2;
NaotoMorita 19:3fae66745363 434 }
NaotoMorita 19:3fae66745363 435
NaotoMorita 23:1509648c2318 436 void ScErrStateEKF::computeVb(Vector3& vb)
NaotoMorita 23:1509648c2318 437 {
NaotoMorita 23:1509648c2318 438 Matrix dcm(3,3);
NaotoMorita 23:1509648c2318 439 computeDcm(dcm, qhat);
NaotoMorita 23:1509648c2318 440 Matrix vbmat = dcm*vihat;
NaotoMorita 23:1509648c2318 441 vb.x = vbmat(1,1);
NaotoMorita 23:1509648c2318 442 vb.y = vbmat(2,1);
NaotoMorita 23:1509648c2318 443 vb.z = vbmat(3,1);
NaotoMorita 23:1509648c2318 444
NaotoMorita 23:1509648c2318 445 }
NaotoMorita 23:1509648c2318 446
NaotoMorita 19:3fae66745363 447
NaotoMorita 19:3fae66745363 448
NaotoMorita 19:3fae66745363 449 Vector3 ScErrStateEKF::calcDynAcc(Vector3 acc, Vector3 accref)
NaotoMorita 19:3fae66745363 450 {
NaotoMorita 19:3fae66745363 451 Matrix dcm(3,3);
NaotoMorita 19:3fae66745363 452 computeDcm(dcm, qhat);
NaotoMorita 19:3fae66745363 453 float _x, _y, _z;
NaotoMorita 19:3fae66745363 454 _x = acc.x-(dcm( 1, 1 )*accref.x+dcm( 1, 2 )*accref.y+dcm( 1, 3 )*accref.z);
NaotoMorita 19:3fae66745363 455 _y = acc.y-(dcm( 2, 1 )*accref.x+dcm( 2, 2 )*accref.y+dcm( 2, 3 )*accref.z);
NaotoMorita 19:3fae66745363 456 _z = acc.z-(dcm( 3, 1 )*accref.x+dcm( 3, 2 )*accref.y+dcm( 3, 3 )*accref.z);
NaotoMorita 19:3fae66745363 457 return Vector3(_x, _y, _z);
NaotoMorita 19:3fae66745363 458 }
NaotoMorita 19:3fae66745363 459
NaotoMorita 19:3fae66745363 460
NaotoMorita 19:3fae66745363 461 bool ScErrStateEKF::determinDynStatus(Vector3 acc, Vector3 accref)
NaotoMorita 19:3fae66745363 462 {
NaotoMorita 19:3fae66745363 463 histffunc[histffuncindex] = acc.Norm()*acc.Norm()-accref.Norm()*accref.Norm()-3.0f*sigma2a;
NaotoMorita 19:3fae66745363 464 if(histffuncindex<9){
NaotoMorita 19:3fae66745363 465 histffuncindex += 1;
NaotoMorita 19:3fae66745363 466 }else{
NaotoMorita 19:3fae66745363 467 histffuncindex =0;
NaotoMorita 19:3fae66745363 468 }
NaotoMorita 19:3fae66745363 469 aveffunc = 0;
NaotoMorita 19:3fae66745363 470 for(int i = 1;i<10;i++){
NaotoMorita 19:3fae66745363 471 aveffunc += 1.0f/10.0f*histffunc[i];
NaotoMorita 19:3fae66745363 472 }
NaotoMorita 19:3fae66745363 473 sigma2f = 1.0f/10.0f*(6.0f*sigma2a*sigma2a+4.0f*accref.Norm()*accref.Norm()*sigma2a);
NaotoMorita 19:3fae66745363 474
NaotoMorita 19:3fae66745363 475
NaotoMorita 19:3fae66745363 476 dynacc = calcDynAcc(acc,accref);
NaotoMorita 19:3fae66745363 477 bool dynCase = true;
NaotoMorita 19:3fae66745363 478 if((dynacc.Norm()<0.005f) && (abs(acc.Norm()-accref.Norm())<0.005f)){dynCase = false;}
NaotoMorita 19:3fae66745363 479 if(aveffunc<sqrt(sigma2f/0.99f) && abs(acc.Norm()-accref.Norm())<0.001f){dynCase = false;}
NaotoMorita 19:3fae66745363 480 return dynCase;
NaotoMorita 19:3fae66745363 481
NaotoMorita 19:3fae66745363 482 }
NaotoMorita 19:3fae66745363 483
NaotoMorita 22:7d84b8bc20b4 484 /*
NaotoMorita 22:7d84b8bc20b4 485 void ScErrStateEKF::updateNominal(Vector3 gyro, Vector3 acc, Vector3 accref, float att_dt)
NaotoMorita 22:7d84b8bc20b4 486 {
NaotoMorita 22:7d84b8bc20b4 487 gyro -= gyroBias;
NaotoMorita 22:7d84b8bc20b4 488 acc -= accBias;
NaotoMorita 22:7d84b8bc20b4 489 Matrix A(4,4);
NaotoMorita 22:7d84b8bc20b4 490 A << 0.0f << -0.5f*gyro.x <<-0.5f*gyro.y <<-0.5f*gyro.z
NaotoMorita 22:7d84b8bc20b4 491 << 0.5f*gyro.x << 0.0f << 0.5f*gyro.z <<-0.5f*gyro.y
NaotoMorita 22:7d84b8bc20b4 492 << 0.5f*gyro.y << -0.5f*gyro.z << 0.0f << 0.5f*gyro.x
NaotoMorita 22:7d84b8bc20b4 493 << 0.5f*gyro.z << 0.5f*gyro.y <<-0.5f*gyro.x << 0.0f ;
NaotoMorita 22:7d84b8bc20b4 494
NaotoMorita 22:7d84b8bc20b4 495 Matrix phi = MatrixMath::Eye(4) + A * att_dt + (0.5f*att_dt*att_dt) * A * A;
NaotoMorita 22:7d84b8bc20b4 496 qhat = phi * qhat;
NaotoMorita 22:7d84b8bc20b4 497 float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qhat),qhat));
NaotoMorita 22:7d84b8bc20b4 498 qhat *= (1.0f/ qnorm);
NaotoMorita 22:7d84b8bc20b4 499
NaotoMorita 22:7d84b8bc20b4 500 Matrix dcm(3,3);
NaotoMorita 22:7d84b8bc20b4 501 computeDcm(dcm, qhat);
NaotoMorita 22:7d84b8bc20b4 502 vihat += (MatrixMath::Transpose(dcm)*MatrixMath::Vector2mat(acc)-MatrixMath::Vector2mat(accref))*att_dt*9.8f;
NaotoMorita 22:7d84b8bc20b4 503 }
NaotoMorita 19:3fae66745363 504
NaotoMorita 22:7d84b8bc20b4 505 void ScErrStateEKF::updateErrState(Vector3 gyro,Vector3 acc, float att_dt)
NaotoMorita 22:7d84b8bc20b4 506 {
NaotoMorita 22:7d84b8bc20b4 507 gyro -= gyroBias;
NaotoMorita 22:7d84b8bc20b4 508 acc -= accBias;
NaotoMorita 22:7d84b8bc20b4 509 Matrix A(nState,nState);
NaotoMorita 22:7d84b8bc20b4 510 A(1,2) = gyro.z;
NaotoMorita 22:7d84b8bc20b4 511 A(1,3) = -gyro.y;
NaotoMorita 22:7d84b8bc20b4 512 A(2,1) = -gyro.z;
NaotoMorita 22:7d84b8bc20b4 513 A(2,3) = gyro.x;
NaotoMorita 22:7d84b8bc20b4 514 A(3,1) = gyro.y;
NaotoMorita 22:7d84b8bc20b4 515 A(3,2) = -gyro.x;
NaotoMorita 22:7d84b8bc20b4 516 A(1,4) = -0.5f;
NaotoMorita 22:7d84b8bc20b4 517 A(2,5) = -0.5f;
NaotoMorita 22:7d84b8bc20b4 518 A(3,6) = -0.5f;
NaotoMorita 22:7d84b8bc20b4 519
NaotoMorita 22:7d84b8bc20b4 520 Matrix dcm(3,3);
NaotoMorita 22:7d84b8bc20b4 521 computeDcm(dcm, qhat);
NaotoMorita 22:7d84b8bc20b4 522 Matrix qeterm = -2.0f*9.8f*MatrixMath::Transpose(dcm)*MatrixMath::Matrixcross(acc.x,acc.y,acc.z);
NaotoMorita 22:7d84b8bc20b4 523 Matrix baterm = -9.8f*MatrixMath::Transpose(dcm);
NaotoMorita 22:7d84b8bc20b4 524 for (int i = 1; i < 4; i++){
NaotoMorita 22:7d84b8bc20b4 525 for (int j = 1; i < 4; i++){
NaotoMorita 22:7d84b8bc20b4 526 A(i+9,j) = qeterm(i,j);
NaotoMorita 22:7d84b8bc20b4 527 A(i+9,j+6) = baterm(i,j);
NaotoMorita 22:7d84b8bc20b4 528 }
NaotoMorita 22:7d84b8bc20b4 529 }
NaotoMorita 22:7d84b8bc20b4 530
NaotoMorita 22:7d84b8bc20b4 531 Matrix phi = MatrixMath::Eye(nState) + A * att_dt + (0.5f*att_dt*att_dt) * A * A;
NaotoMorita 22:7d84b8bc20b4 532 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 533 errState = phi * errState;
NaotoMorita 22:7d84b8bc20b4 534 Phat = phi*Phat*MatrixMath::Transpose(phi)+Qd;
NaotoMorita 22:7d84b8bc20b4 535 }
NaotoMorita 22:7d84b8bc20b4 536
NaotoMorita 22:7d84b8bc20b4 537 void ScErrStateEKF::updateVelocityConstraints()
NaotoMorita 22:7d84b8bc20b4 538 {
NaotoMorita 22:7d84b8bc20b4 539
NaotoMorita 22:7d84b8bc20b4 540 Matrix dcm(3,3);
NaotoMorita 22:7d84b8bc20b4 541 computeDcm(dcm, qhat);
NaotoMorita 22:7d84b8bc20b4 542 Matrix H(2,nState);
NaotoMorita 22:7d84b8bc20b4 543
NaotoMorita 22:7d84b8bc20b4 544 Matrix nomVb = dcm*vihat;
NaotoMorita 22:7d84b8bc20b4 545 Matrix qeterm = -2.0f*MatrixMath::Matrixcross(nomVb(1,1),nomVb(2,1),nomVb(3,1));
NaotoMorita 22:7d84b8bc20b4 546 for(int j = 1;j<4;j++){
NaotoMorita 22:7d84b8bc20b4 547 H(1,j) = -qeterm(1,j);
NaotoMorita 22:7d84b8bc20b4 548 H(2,j) = -qeterm(3,j);
NaotoMorita 22:7d84b8bc20b4 549 //H(3,j) = -qeterm(3,j);
NaotoMorita 22:7d84b8bc20b4 550 H(1,9+j) = -dcm(1,j);
NaotoMorita 22:7d84b8bc20b4 551 H(2,9+j) = -dcm(3,j);
NaotoMorita 22:7d84b8bc20b4 552 //H(3,9+j) = -dcm(3,j);
NaotoMorita 22:7d84b8bc20b4 553 }
NaotoMorita 22:7d84b8bc20b4 554
NaotoMorita 22:7d84b8bc20b4 555 Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+Rvsc);
NaotoMorita 22:7d84b8bc20b4 556 Matrix z(2,1);
NaotoMorita 22:7d84b8bc20b4 557 z << nomVb(1,1) << nomVb(3,1) ;
NaotoMorita 22:7d84b8bc20b4 558 Matrix corrVal = K * (z-H*errState);
NaotoMorita 22:7d84b8bc20b4 559 errState = errState + corrVal;
NaotoMorita 22:7d84b8bc20b4 560 Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*(Rvsc)*MatrixMath::Transpose(K);
NaotoMorita 22:7d84b8bc20b4 561
NaotoMorita 22:7d84b8bc20b4 562 }
NaotoMorita 22:7d84b8bc20b4 563
NaotoMorita 22:7d84b8bc20b4 564 void ScErrStateEKF::updateConstraints(Vector3 gyro)
NaotoMorita 22:7d84b8bc20b4 565 {
NaotoMorita 22:7d84b8bc20b4 566
NaotoMorita 22:7d84b8bc20b4 567 gyro -=gyroBias;
NaotoMorita 22:7d84b8bc20b4 568
NaotoMorita 22:7d84b8bc20b4 569 Matrix dcm(3,3);
NaotoMorita 22:7d84b8bc20b4 570 computeDcm(dcm, qhat);
NaotoMorita 22:7d84b8bc20b4 571
NaotoMorita 22:7d84b8bc20b4 572 Matrix qeterm = -2.0f*MatrixMath::Transpose(dcm)*MatrixMath::Matrixcross(gyro.x,gyro.y,gyro.z);
NaotoMorita 22:7d84b8bc20b4 573 Matrix H(4,nState);
NaotoMorita 22:7d84b8bc20b4 574 for(int i = 1; i<3; i++){
NaotoMorita 22:7d84b8bc20b4 575 for(int j = 1;j<4;j++){
NaotoMorita 22:7d84b8bc20b4 576 H(i,j) = qeterm(i,j);
NaotoMorita 22:7d84b8bc20b4 577 }
NaotoMorita 22:7d84b8bc20b4 578 }
NaotoMorita 22:7d84b8bc20b4 579 H(1,4) = 1.0f*(dcm(1,1));
NaotoMorita 22:7d84b8bc20b4 580 H(1,5) = 1.0f*(dcm(2,1));
NaotoMorita 22:7d84b8bc20b4 581 H(1,6) = 1.0f*(dcm(3,1));
NaotoMorita 22:7d84b8bc20b4 582 H(2,4) = 1.0f*(dcm(1,2));
NaotoMorita 22:7d84b8bc20b4 583 H(2,5) = 1.0f*(dcm(2,2));
NaotoMorita 22:7d84b8bc20b4 584 H(2,6) = 1.0f*(dcm(3,2));
NaotoMorita 22:7d84b8bc20b4 585
NaotoMorita 22:7d84b8bc20b4 586 Matrix nomVb = dcm*vihat;
NaotoMorita 22:7d84b8bc20b4 587 qeterm = -2.0f*MatrixMath::Matrixcross(nomVb(1,1),nomVb(2,1),nomVb(3,1));
NaotoMorita 22:7d84b8bc20b4 588 for(int j = 1;j<4;j++){
NaotoMorita 22:7d84b8bc20b4 589 H(3,j) = -qeterm(1,j);
NaotoMorita 22:7d84b8bc20b4 590 H(4,j) = -qeterm(3,j);
NaotoMorita 22:7d84b8bc20b4 591 //H(3,j) = -qeterm(3,j);
NaotoMorita 22:7d84b8bc20b4 592 H(3,9+j) = -dcm(1,j);
NaotoMorita 22:7d84b8bc20b4 593 H(4,9+j) = -dcm(3,j);
NaotoMorita 22:7d84b8bc20b4 594 //H(3,9+j) = -dcm(3,j);
NaotoMorita 22:7d84b8bc20b4 595 }
NaotoMorita 22:7d84b8bc20b4 596
NaotoMorita 22:7d84b8bc20b4 597 Matrix R(4,4);
NaotoMorita 22:7d84b8bc20b4 598 R(1,1) = Rgsc(1,1);
NaotoMorita 22:7d84b8bc20b4 599 R(2,2) = Rgsc(2,1);
NaotoMorita 22:7d84b8bc20b4 600 R(3,3) = Rvsc(1,1);
NaotoMorita 22:7d84b8bc20b4 601 R(4,4) = Rvsc(2,1);
NaotoMorita 22:7d84b8bc20b4 602 //R(5,5) = Rvsc(3,1);
NaotoMorita 22:7d84b8bc20b4 603 Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
NaotoMorita 22:7d84b8bc20b4 604 Matrix z(4,1);
NaotoMorita 22:7d84b8bc20b4 605 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 606 Matrix corrVal = K * (z-H*errState);
NaotoMorita 22:7d84b8bc20b4 607 errState = errState + corrVal;
NaotoMorita 22:7d84b8bc20b4 608 Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*(R)*MatrixMath::Transpose(K);
NaotoMorita 22:7d84b8bc20b4 609 }
NaotoMorita 22:7d84b8bc20b4 610
NaotoMorita 22:7d84b8bc20b4 611 void ScErrStateEKF::computeVb(Vector3& vb)
NaotoMorita 22:7d84b8bc20b4 612 {
NaotoMorita 22:7d84b8bc20b4 613 Matrix dcm(3,3);
NaotoMorita 22:7d84b8bc20b4 614 computeDcm(dcm, qhat);
NaotoMorita 22:7d84b8bc20b4 615 Matrix vbmat = dcm*vihat;
NaotoMorita 22:7d84b8bc20b4 616 vb.x = vbmat(1,1);
NaotoMorita 22:7d84b8bc20b4 617 vb.y = vbmat(2,1);
NaotoMorita 22:7d84b8bc20b4 618 vb.z = vbmat(3,1);
NaotoMorita 22:7d84b8bc20b4 619
NaotoMorita 22:7d84b8bc20b4 620 }
NaotoMorita 26:73c3f58b9d70 621
NaotoMorita 26:73c3f58b9d70 622 void ScErrStateEKF::updateMagMeasures(Vector3 mag)
NaotoMorita 26:73c3f58b9d70 623 {
NaotoMorita 26:73c3f58b9d70 624 //mag = mag/mag.Norm();
NaotoMorita 26:73c3f58b9d70 625 Matrix dcm(3,3);
NaotoMorita 26:73c3f58b9d70 626 computeDcm(dcm, qhat);
NaotoMorita 26:73c3f58b9d70 627
NaotoMorita 26:73c3f58b9d70 628 Matrix magvec(3,1);
NaotoMorita 26:73c3f58b9d70 629 magvec(1,1) = mag.x;
NaotoMorita 26:73c3f58b9d70 630 magvec(2,1) = mag.y;
NaotoMorita 26:73c3f58b9d70 631 magvec(3,1) = mag.z;
NaotoMorita 26:73c3f58b9d70 632
NaotoMorita 26:73c3f58b9d70 633 Matrix magnedvec = MatrixMath::Transpose(dcm)*magvec;
NaotoMorita 26:73c3f58b9d70 634 Matrix magrefmod(3,1);
NaotoMorita 26:73c3f58b9d70 635 magrefmod(1,1) = sqrt(magnedvec(1,1)*magnedvec(1,1)+magnedvec(2,1)*magnedvec(2,1));
NaotoMorita 26:73c3f58b9d70 636 magrefmod(2,1) = 0.0f;
NaotoMorita 26:73c3f58b9d70 637 magrefmod(3,1) = magnedvec(3,1);
NaotoMorita 26:73c3f58b9d70 638
NaotoMorita 26:73c3f58b9d70 639 Matrix mvec = dcm*magrefmod ;
NaotoMorita 26:73c3f58b9d70 640 Matrix H(3,nState);
NaotoMorita 26:73c3f58b9d70 641 H(1,2) = -2.0f*magvec(3,1);
NaotoMorita 26:73c3f58b9d70 642 H(1,3) = 2.0f*magvec(2,1);
NaotoMorita 26:73c3f58b9d70 643 H(2,1) = 2.0f*magvec(3,1);
NaotoMorita 26:73c3f58b9d70 644 H(2,3) = -2.0f*magvec(1,1);
NaotoMorita 26:73c3f58b9d70 645 H(3,1) = -2.0f*magvec(2,1);
NaotoMorita 26:73c3f58b9d70 646 H(3,2) = 2.0f*magvec(1,1);
NaotoMorita 26:73c3f58b9d70 647 Matrix Pm(nState,nState);
NaotoMorita 26:73c3f58b9d70 648 for(int i = 1; i<4; i++){
NaotoMorita 26:73c3f58b9d70 649 for(int j = 1;j<4;j++){
NaotoMorita 26:73c3f58b9d70 650 Pm(i,j) = Phat(i,j);
NaotoMorita 26:73c3f58b9d70 651 }
NaotoMorita 26:73c3f58b9d70 652 }
NaotoMorita 26:73c3f58b9d70 653 Matrix r3(3,1);
NaotoMorita 26:73c3f58b9d70 654 r3 << dcm(1,3)<< dcm(2,3) << dcm(3,3);
NaotoMorita 26:73c3f58b9d70 655 Matrix kpart = r3*MatrixMath::Transpose(r3);
NaotoMorita 26:73c3f58b9d70 656 Matrix Kmod(nState,nState);
NaotoMorita 26:73c3f58b9d70 657 for(int i = 1; i<4; i++){
NaotoMorita 26:73c3f58b9d70 658 for(int j = 1;j<4;j++){
NaotoMorita 26:73c3f58b9d70 659 Kmod(i,j) = kpart(i,j);
NaotoMorita 26:73c3f58b9d70 660 }
NaotoMorita 26:73c3f58b9d70 661 }
NaotoMorita 26:73c3f58b9d70 662
NaotoMorita 26:73c3f58b9d70 663 Matrix K = Kmod*(Pm*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Pm*MatrixMath::Transpose(H)+Rm);
NaotoMorita 26:73c3f58b9d70 664 Matrix z = magvec-mvec;
NaotoMorita 26:73c3f58b9d70 665 errState = errState + K * (z-H*errState);
NaotoMorita 26:73c3f58b9d70 666 Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*(Rm)*MatrixMath::Transpose(K);
NaotoMorita 26:73c3f58b9d70 667 }
NaotoMorita 26:73c3f58b9d70 668
NaotoMorita 26:73c3f58b9d70 669 void ScErrStateEKF::updateVelocityConstraints()
NaotoMorita 26:73c3f58b9d70 670 {
NaotoMorita 26:73c3f58b9d70 671
NaotoMorita 26:73c3f58b9d70 672 Matrix dcm(3,3);
NaotoMorita 26:73c3f58b9d70 673 computeDcm(dcm, qhat);
NaotoMorita 26:73c3f58b9d70 674 Matrix H(2,nState);
NaotoMorita 26:73c3f58b9d70 675
NaotoMorita 26:73c3f58b9d70 676 Matrix nomVb = dcm*vihat;
NaotoMorita 26:73c3f58b9d70 677 Matrix qeterm = -2.0f*MatrixMath::Matrixcross(nomVb(1,1),nomVb(2,1),nomVb(3,1));
NaotoMorita 26:73c3f58b9d70 678 for(int j = 1;j<4;j++){
NaotoMorita 26:73c3f58b9d70 679 H(1,j) = -qeterm(1,j);
NaotoMorita 26:73c3f58b9d70 680 H(2,j) = -qeterm(3,j);
NaotoMorita 26:73c3f58b9d70 681 //H(3,j) = -qeterm(3,j);
NaotoMorita 26:73c3f58b9d70 682 H(1,9+j) = -dcm(1,j);
NaotoMorita 26:73c3f58b9d70 683 H(2,9+j) = -dcm(3,j);
NaotoMorita 26:73c3f58b9d70 684 //H(3,9+j) = -dcm(3,j);
NaotoMorita 26:73c3f58b9d70 685 }
NaotoMorita 26:73c3f58b9d70 686
NaotoMorita 26:73c3f58b9d70 687 Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+Rvsc);
NaotoMorita 26:73c3f58b9d70 688 Matrix z(2,1);
NaotoMorita 26:73c3f58b9d70 689 z << nomVb(1,1) << nomVb(3,1) ;
NaotoMorita 26:73c3f58b9d70 690 Matrix corrVal = K * (z-H*errState);
NaotoMorita 26:73c3f58b9d70 691 errState = errState + corrVal;
NaotoMorita 26:73c3f58b9d70 692 Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*(Rvsc)*MatrixMath::Transpose(K);
NaotoMorita 26:73c3f58b9d70 693
NaotoMorita 26:73c3f58b9d70 694 }
NaotoMorita 22:7d84b8bc20b4 695 */
NaotoMorita 22:7d84b8bc20b4 696
NaotoMorita 22:7d84b8bc20b4 697