Eigen
Dependencies: Eigen
Dependents: optWingforHAPS_Eigen hexaTest_Eigen
ScErrStateEKF.cpp@25:07ac5c6cd61c, 2021-08-25 (annotated)
- Committer:
- NaotoMorita
- Date:
- Wed Aug 25 07:01:05 2021 +0000
- Revision:
- 25:07ac5c6cd61c
- Parent:
- 23:1509648c2318
- Child:
- 26:73c3f58b9d70
centered correction
Who changed what in which revision?
User | Revision | Line number | New 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 | 23:1509648c2318 | 11 | :qhat(4,1),vihat(3,1), errState(12,1), Phat(12,12), Q(12,12), Ra(3,3), 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 | 21:d6079def0473 | 16 | setDiag(Phat,0.1f); |
NaotoMorita | 21:d6079def0473 | 17 | setQqerr(0.000005f); |
NaotoMorita | 21:d6079def0473 | 18 | setQgbias(0.00001f); |
NaotoMorita | 23:1509648c2318 | 19 | setQabias(1.0f); |
NaotoMorita | 23:1509648c2318 | 20 | setQv(0.000001f); |
NaotoMorita | 19:3fae66745363 | 21 | |
NaotoMorita | 20:37d3c3ee36e9 | 22 | //加速度の観測 |
NaotoMorita | 23:1509648c2318 | 23 | setDiag(Ra,0.005f); |
NaotoMorita | 23:1509648c2318 | 24 | setDiag(Qab,100.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 | 23:1509648c2318 | 30 | setDiag(Rvsc,100000.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 | 23:1509648c2318 | 43 | void ScErrStateEKF::updateNominal(Vector3 gyro, Vector3 acc, Vector3 accref, float att_dt) |
NaotoMorita | 19:3fae66745363 | 44 | { |
NaotoMorita | 19:3fae66745363 | 45 | gyro -= gyroBias; |
NaotoMorita | 23:1509648c2318 | 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 | 23:1509648c2318 | 57 | |
NaotoMorita | 23:1509648c2318 | 58 | Matrix dcm(3,3); |
NaotoMorita | 23:1509648c2318 | 59 | computeDcm(dcm, qhat); |
NaotoMorita | 23:1509648c2318 | 60 | vihat += (MatrixMath::Transpose(dcm)*MatrixMath::Vector2mat(acc)-MatrixMath::Vector2mat(accref))*att_dt*9.8f; |
NaotoMorita | 19:3fae66745363 | 61 | } |
NaotoMorita | 19:3fae66745363 | 62 | |
NaotoMorita | 23:1509648c2318 | 63 | void ScErrStateEKF::updateErrState(Vector3 gyro,Vector3 acc, float att_dt) |
NaotoMorita | 23:1509648c2318 | 64 | { |
NaotoMorita | 23:1509648c2318 | 65 | gyro -= gyroBias; |
NaotoMorita | 23:1509648c2318 | 66 | acc -= accBias; |
NaotoMorita | 23:1509648c2318 | 67 | Matrix A(nState,nState); |
NaotoMorita | 23:1509648c2318 | 68 | A(1,2) = gyro.z; |
NaotoMorita | 23:1509648c2318 | 69 | A(1,3) = -gyro.y; |
NaotoMorita | 23:1509648c2318 | 70 | A(2,1) = -gyro.z; |
NaotoMorita | 23:1509648c2318 | 71 | A(2,3) = gyro.x; |
NaotoMorita | 23:1509648c2318 | 72 | A(3,1) = gyro.y; |
NaotoMorita | 23:1509648c2318 | 73 | A(3,2) = -gyro.x; |
NaotoMorita | 23:1509648c2318 | 74 | A(1,4) = -0.5f; |
NaotoMorita | 23:1509648c2318 | 75 | A(2,5) = -0.5f; |
NaotoMorita | 23:1509648c2318 | 76 | A(3,6) = -0.5f; |
NaotoMorita | 23:1509648c2318 | 77 | |
NaotoMorita | 23:1509648c2318 | 78 | Matrix dcm(3,3); |
NaotoMorita | 23:1509648c2318 | 79 | computeDcm(dcm, qhat); |
NaotoMorita | 23:1509648c2318 | 80 | Matrix qeterm = -2.0f*9.8f*MatrixMath::Transpose(dcm)*MatrixMath::Matrixcross(acc.x,acc.y,acc.z); |
NaotoMorita | 23:1509648c2318 | 81 | Matrix baterm = -9.8f*MatrixMath::Transpose(dcm); |
NaotoMorita | 23:1509648c2318 | 82 | for (int i = 1; i < 4; i++){ |
NaotoMorita | 23:1509648c2318 | 83 | for (int j = 1; i < 4; i++){ |
NaotoMorita | 23:1509648c2318 | 84 | A(i+9,j) = qeterm(i,j); |
NaotoMorita | 23:1509648c2318 | 85 | A(i+9,j+6) = baterm(i,j); |
NaotoMorita | 23:1509648c2318 | 86 | } |
NaotoMorita | 23:1509648c2318 | 87 | } |
NaotoMorita | 23:1509648c2318 | 88 | |
NaotoMorita | 23:1509648c2318 | 89 | Matrix phi = MatrixMath::Eye(nState) + A * att_dt + (0.5f*att_dt*att_dt) * A * A; |
NaotoMorita | 23:1509648c2318 | 90 | 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 | 91 | errState = phi * errState; |
NaotoMorita | 23:1509648c2318 | 92 | Phat = phi*Phat*MatrixMath::Transpose(phi)+Qd; |
NaotoMorita | 23:1509648c2318 | 93 | } |
NaotoMorita | 20:37d3c3ee36e9 | 94 | void ScErrStateEKF::setQqerr(float val){ |
NaotoMorita | 20:37d3c3ee36e9 | 95 | Q(1,1) = val; |
NaotoMorita | 20:37d3c3ee36e9 | 96 | Q(2,2) = val; |
NaotoMorita | 20:37d3c3ee36e9 | 97 | Q(3,3) = val; |
NaotoMorita | 20:37d3c3ee36e9 | 98 | } |
NaotoMorita | 20:37d3c3ee36e9 | 99 | |
NaotoMorita | 20:37d3c3ee36e9 | 100 | void ScErrStateEKF::setQgbias(float val){ |
NaotoMorita | 20:37d3c3ee36e9 | 101 | Q(4,4) = val; |
NaotoMorita | 20:37d3c3ee36e9 | 102 | Q(5,5) = val; |
NaotoMorita | 20:37d3c3ee36e9 | 103 | Q(6,6) = val; |
NaotoMorita | 20:37d3c3ee36e9 | 104 | } |
NaotoMorita | 20:37d3c3ee36e9 | 105 | void ScErrStateEKF::setQabias(float val){ |
NaotoMorita | 20:37d3c3ee36e9 | 106 | Q(7,7) = val; |
NaotoMorita | 20:37d3c3ee36e9 | 107 | Q(8,8) = val; |
NaotoMorita | 20:37d3c3ee36e9 | 108 | Q(9,9) = val; |
NaotoMorita | 20:37d3c3ee36e9 | 109 | } |
NaotoMorita | 23:1509648c2318 | 110 | void ScErrStateEKF::setQv(float val){ |
NaotoMorita | 23:1509648c2318 | 111 | Q(10,10) = val; |
NaotoMorita | 23:1509648c2318 | 112 | Q(11,11) = val; |
NaotoMorita | 23:1509648c2318 | 113 | Q(12,12) = val; |
NaotoMorita | 23:1509648c2318 | 114 | } |
NaotoMorita | 21:d6079def0473 | 115 | |
NaotoMorita | 20:37d3c3ee36e9 | 116 | void ScErrStateEKF::setQab(float val){ |
NaotoMorita | 21:d6079def0473 | 117 | setDiag(Qab,val); |
NaotoMorita | 20:37d3c3ee36e9 | 118 | } |
NaotoMorita | 20:37d3c3ee36e9 | 119 | |
NaotoMorita | 23:1509648c2318 | 120 | |
NaotoMorita | 20:37d3c3ee36e9 | 121 | void ScErrStateEKF::setRsoftconst(float Vgsc,float Vvsc){ |
NaotoMorita | 21:d6079def0473 | 122 | setDiag(Rgsc,Vgsc); |
NaotoMorita | 23:1509648c2318 | 123 | setDiag(Rvsc,Vvsc); |
NaotoMorita | 20:37d3c3ee36e9 | 124 | } |
NaotoMorita | 20:37d3c3ee36e9 | 125 | |
NaotoMorita | 21:d6079def0473 | 126 | void ScErrStateEKF::setDiag(Matrix& mat, float val){ |
NaotoMorita | 21:d6079def0473 | 127 | for (int i = 1; i < mat.getCols()+1; i++){ |
NaotoMorita | 21:d6079def0473 | 128 | mat(i,i) = val; |
NaotoMorita | 21:d6079def0473 | 129 | } |
NaotoMorita | 21:d6079def0473 | 130 | } |
NaotoMorita | 20:37d3c3ee36e9 | 131 | |
NaotoMorita | 25:07ac5c6cd61c | 132 | void ScErrStateEKF::getQhat(float (&res)[4]){ |
NaotoMorita | 25:07ac5c6cd61c | 133 | for (int i = 0; i < 4; i++){ |
NaotoMorita | 25:07ac5c6cd61c | 134 | res[i] = qhat(i+1,1); |
NaotoMorita | 25:07ac5c6cd61c | 135 | } |
NaotoMorita | 25:07ac5c6cd61c | 136 | } |
NaotoMorita | 25:07ac5c6cd61c | 137 | |
NaotoMorita | 25:07ac5c6cd61c | 138 | void ScErrStateEKF::getVihat(float (&res)[3]){ |
NaotoMorita | 25:07ac5c6cd61c | 139 | for (int i = 0; i < 3; i++){ |
NaotoMorita | 25:07ac5c6cd61c | 140 | res[i] = vihat(i+1,1); |
NaotoMorita | 25:07ac5c6cd61c | 141 | } |
NaotoMorita | 25:07ac5c6cd61c | 142 | } |
NaotoMorita | 25:07ac5c6cd61c | 143 | |
NaotoMorita | 25:07ac5c6cd61c | 144 | void ScErrStateEKF::getGyroBias(float (&resVal)[3], float (&resCov)[6]){ |
NaotoMorita | 25:07ac5c6cd61c | 145 | resVal[0] = gyroBias.x; |
NaotoMorita | 25:07ac5c6cd61c | 146 | resVal[1] = gyroBias.y; |
NaotoMorita | 25:07ac5c6cd61c | 147 | resVal[2] = gyroBias.z; |
NaotoMorita | 25:07ac5c6cd61c | 148 | resCov[0] = Phat(4,4); |
NaotoMorita | 25:07ac5c6cd61c | 149 | resCov[1] = Phat(4,5); |
NaotoMorita | 25:07ac5c6cd61c | 150 | resCov[2] = Phat(4,6); |
NaotoMorita | 25:07ac5c6cd61c | 151 | resCov[3] = Phat(5,5); |
NaotoMorita | 25:07ac5c6cd61c | 152 | resCov[4] = Phat(5,6); |
NaotoMorita | 25:07ac5c6cd61c | 153 | resCov[5] = Phat(6,6); |
NaotoMorita | 25:07ac5c6cd61c | 154 | |
NaotoMorita | 25:07ac5c6cd61c | 155 | } |
NaotoMorita | 25:07ac5c6cd61c | 156 | |
NaotoMorita | 25:07ac5c6cd61c | 157 | void ScErrStateEKF::getAccBias(float (&resVal)[3], float (&resCov)[6]){ |
NaotoMorita | 25:07ac5c6cd61c | 158 | resVal[0] = accBias.x; |
NaotoMorita | 25:07ac5c6cd61c | 159 | resVal[1] = accBias.y; |
NaotoMorita | 25:07ac5c6cd61c | 160 | resVal[2] = accBias.z; |
NaotoMorita | 25:07ac5c6cd61c | 161 | resCov[0] = Phat(7,7); |
NaotoMorita | 25:07ac5c6cd61c | 162 | resCov[1] = Phat(7,8); |
NaotoMorita | 25:07ac5c6cd61c | 163 | resCov[2] = Phat(7,9); |
NaotoMorita | 25:07ac5c6cd61c | 164 | resCov[3] = Phat(8,8); |
NaotoMorita | 25:07ac5c6cd61c | 165 | resCov[4] = Phat(8,9); |
NaotoMorita | 25:07ac5c6cd61c | 166 | resCov[5] = Phat(9,9); |
NaotoMorita | 25:07ac5c6cd61c | 167 | |
NaotoMorita | 25:07ac5c6cd61c | 168 | } |
NaotoMorita | 25:07ac5c6cd61c | 169 | |
NaotoMorita | 19:3fae66745363 | 170 | |
NaotoMorita | 19:3fae66745363 | 171 | void ScErrStateEKF::updateAccMeasures(Vector3 acc, Vector3 accref) |
NaotoMorita | 19:3fae66745363 | 172 | { |
NaotoMorita | 21:d6079def0473 | 173 | acc -= accBias; |
NaotoMorita | 20:37d3c3ee36e9 | 174 | Matrix dcm(3,3); |
NaotoMorita | 20:37d3c3ee36e9 | 175 | computeDcm(dcm, qhat); |
NaotoMorita | 20:37d3c3ee36e9 | 176 | Vector3 gvec(dcm(1,3)*accref.z, dcm(2,3)*accref.z, dcm(3,3)*accref.z); |
NaotoMorita | 21:d6079def0473 | 177 | Matrix H(3,nState); |
NaotoMorita | 20:37d3c3ee36e9 | 178 | H(1,2) = -2.0f*gvec.z; |
NaotoMorita | 20:37d3c3ee36e9 | 179 | H(1,3) = 2.0f*gvec.y; |
NaotoMorita | 20:37d3c3ee36e9 | 180 | H(2,1) = 2.0f*gvec.z; |
NaotoMorita | 20:37d3c3ee36e9 | 181 | H(2,3) = -2.0f*gvec.x; |
NaotoMorita | 20:37d3c3ee36e9 | 182 | H(3,1) = -2.0f*gvec.y; |
NaotoMorita | 20:37d3c3ee36e9 | 183 | H(3,2) = 2.0f*gvec.x; |
NaotoMorita | 20:37d3c3ee36e9 | 184 | H(1,7) = 1.0f; |
NaotoMorita | 20:37d3c3ee36e9 | 185 | H(2,8) = 1.0f; |
NaotoMorita | 20:37d3c3ee36e9 | 186 | H(3,9) = 1.0f; |
NaotoMorita | 20:37d3c3ee36e9 | 187 | Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+Ra+Qab); |
NaotoMorita | 21:d6079def0473 | 188 | Matrix z = MatrixMath::Vector2mat(acc)-dcm*MatrixMath::Vector2mat(accref); |
NaotoMorita | 20:37d3c3ee36e9 | 189 | Matrix corrVal = K * (z-H*errState); |
NaotoMorita | 20:37d3c3ee36e9 | 190 | errState = errState + corrVal; |
NaotoMorita | 21:d6079def0473 | 191 | Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*(Ra+Qab)*MatrixMath::Transpose(K); |
NaotoMorita | 20:37d3c3ee36e9 | 192 | } |
NaotoMorita | 20:37d3c3ee36e9 | 193 | |
NaotoMorita | 20:37d3c3ee36e9 | 194 | void ScErrStateEKF::updateGyroBiasConstraints(Vector3 gyro) |
NaotoMorita | 20:37d3c3ee36e9 | 195 | { |
NaotoMorita | 21:d6079def0473 | 196 | gyro -= gyroBias; |
NaotoMorita | 21:d6079def0473 | 197 | |
NaotoMorita | 20:37d3c3ee36e9 | 198 | Matrix dcm(3,3); |
NaotoMorita | 20:37d3c3ee36e9 | 199 | computeDcm(dcm, qhat); |
NaotoMorita | 23:1509648c2318 | 200 | Matrix qeterm = -2.0f*MatrixMath::Transpose(dcm)*MatrixMath::Matrixcross(gyro.x,gyro.y,gyro.z); |
NaotoMorita | 23:1509648c2318 | 201 | Matrix H(2,nState); |
NaotoMorita | 23:1509648c2318 | 202 | for(int i = 1; i<3; i++){ |
NaotoMorita | 23:1509648c2318 | 203 | for(int j = 1;j<4;j++){ |
NaotoMorita | 23:1509648c2318 | 204 | H(i,j) = qeterm(i,j); |
NaotoMorita | 23:1509648c2318 | 205 | } |
NaotoMorita | 23:1509648c2318 | 206 | } |
NaotoMorita | 23:1509648c2318 | 207 | H(1,4) = 1.0f*(dcm(1,1)); |
NaotoMorita | 23:1509648c2318 | 208 | H(1,5) = 1.0f*(dcm(2,1)); |
NaotoMorita | 23:1509648c2318 | 209 | H(1,6) = 1.0f*(dcm(3,1)); |
NaotoMorita | 23:1509648c2318 | 210 | H(2,4) = 1.0f*(dcm(1,2)); |
NaotoMorita | 23:1509648c2318 | 211 | H(2,5) = 1.0f*(dcm(2,2)); |
NaotoMorita | 23:1509648c2318 | 212 | H(2,6) = 1.0f*(dcm(3,2)); |
NaotoMorita | 23:1509648c2318 | 213 | |
NaotoMorita | 23:1509648c2318 | 214 | Matrix Pm(nState,nState); |
NaotoMorita | 23:1509648c2318 | 215 | for(int i = 4; i<7; i++){ |
NaotoMorita | 23:1509648c2318 | 216 | for(int j = 4;j<7;j++){ |
NaotoMorita | 23:1509648c2318 | 217 | Pm(i,j) = Phat(i,j); |
NaotoMorita | 23:1509648c2318 | 218 | } |
NaotoMorita | 23:1509648c2318 | 219 | } |
NaotoMorita | 23:1509648c2318 | 220 | |
NaotoMorita | 23:1509648c2318 | 221 | Matrix K = (Pm*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Pm*MatrixMath::Transpose(H)+Rgsc); |
NaotoMorita | 23:1509648c2318 | 222 | Matrix z(2,1); |
NaotoMorita | 23:1509648c2318 | 223 | 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 | 23:1509648c2318 | 224 | Matrix corrVal = K * (z-H*errState); |
NaotoMorita | 23:1509648c2318 | 225 | errState = errState + corrVal; |
NaotoMorita | 23:1509648c2318 | 226 | Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*(Rgsc)*MatrixMath::Transpose(K); |
NaotoMorita | 23:1509648c2318 | 227 | } |
NaotoMorita | 23:1509648c2318 | 228 | |
NaotoMorita | 23:1509648c2318 | 229 | void ScErrStateEKF::updateVelocityConstraints() |
NaotoMorita | 23:1509648c2318 | 230 | { |
NaotoMorita | 23:1509648c2318 | 231 | |
NaotoMorita | 23:1509648c2318 | 232 | Matrix dcm(3,3); |
NaotoMorita | 23:1509648c2318 | 233 | computeDcm(dcm, qhat); |
NaotoMorita | 21:d6079def0473 | 234 | Matrix H(2,nState); |
NaotoMorita | 23:1509648c2318 | 235 | |
NaotoMorita | 23:1509648c2318 | 236 | Matrix nomVb = dcm*vihat; |
NaotoMorita | 23:1509648c2318 | 237 | Matrix qeterm = -2.0f*MatrixMath::Matrixcross(nomVb(1,1),nomVb(2,1),nomVb(3,1)); |
NaotoMorita | 23:1509648c2318 | 238 | for(int j = 1;j<4;j++){ |
NaotoMorita | 23:1509648c2318 | 239 | H(1,j) = -qeterm(1,j); |
NaotoMorita | 23:1509648c2318 | 240 | H(2,j) = -qeterm(3,j); |
NaotoMorita | 23:1509648c2318 | 241 | //H(3,j) = -qeterm(3,j); |
NaotoMorita | 23:1509648c2318 | 242 | H(1,9+j) = -dcm(1,j); |
NaotoMorita | 23:1509648c2318 | 243 | H(2,9+j) = -dcm(3,j); |
NaotoMorita | 23:1509648c2318 | 244 | //H(3,9+j) = -dcm(3,j); |
NaotoMorita | 23:1509648c2318 | 245 | } |
NaotoMorita | 23:1509648c2318 | 246 | |
NaotoMorita | 23:1509648c2318 | 247 | Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+Rvsc); |
NaotoMorita | 23:1509648c2318 | 248 | Matrix z(2,1); |
NaotoMorita | 23:1509648c2318 | 249 | z << nomVb(1,1) << nomVb(3,1) ; |
NaotoMorita | 23:1509648c2318 | 250 | Matrix corrVal = K * (z-H*errState); |
NaotoMorita | 23:1509648c2318 | 251 | errState = errState + corrVal; |
NaotoMorita | 23:1509648c2318 | 252 | Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*(Rvsc)*MatrixMath::Transpose(K); |
NaotoMorita | 23:1509648c2318 | 253 | |
NaotoMorita | 23:1509648c2318 | 254 | } |
NaotoMorita | 23:1509648c2318 | 255 | |
NaotoMorita | 23:1509648c2318 | 256 | void ScErrStateEKF::updateMeasures(Vector3 gyro, Vector3 acc, Vector3 accref) |
NaotoMorita | 23:1509648c2318 | 257 | { |
NaotoMorita | 23:1509648c2318 | 258 | acc -= accBias; |
NaotoMorita | 23:1509648c2318 | 259 | gyro -= gyroBias; |
NaotoMorita | 23:1509648c2318 | 260 | Matrix dcm(3,3); |
NaotoMorita | 23:1509648c2318 | 261 | computeDcm(dcm, qhat); |
NaotoMorita | 23:1509648c2318 | 262 | Vector3 gvec(dcm(1,3)*accref.z, dcm(2,3)*accref.z, dcm(3,3)*accref.z); |
NaotoMorita | 23:1509648c2318 | 263 | Matrix H(5,nState); |
NaotoMorita | 23:1509648c2318 | 264 | H(1,2) = -2.0f*gvec.z; |
NaotoMorita | 23:1509648c2318 | 265 | H(1,3) = 2.0f*gvec.y; |
NaotoMorita | 23:1509648c2318 | 266 | H(2,1) = 2.0f*gvec.z; |
NaotoMorita | 23:1509648c2318 | 267 | H(2,3) = -2.0f*gvec.x; |
NaotoMorita | 23:1509648c2318 | 268 | H(3,1) = -2.0f*gvec.y; |
NaotoMorita | 23:1509648c2318 | 269 | H(3,2) = 2.0f*gvec.x; |
NaotoMorita | 23:1509648c2318 | 270 | H(1,7) = 1.0f; |
NaotoMorita | 23:1509648c2318 | 271 | H(2,8) = 1.0f; |
NaotoMorita | 23:1509648c2318 | 272 | H(3,9) = 1.0f; |
NaotoMorita | 23:1509648c2318 | 273 | /* |
NaotoMorita | 23:1509648c2318 | 274 | Matrix qeterm = -2.0f*MatrixMath::Transpose(dcm)*MatrixMath::Matrixcross(gyro.x,gyro.y,gyro.z); |
NaotoMorita | 23:1509648c2318 | 275 | for(int j = 1;j<4;j++){ |
NaotoMorita | 23:1509648c2318 | 276 | H(4,j) = qeterm(1,j); |
NaotoMorita | 23:1509648c2318 | 277 | H(5,j) = qeterm(2,j); |
NaotoMorita | 23:1509648c2318 | 278 | } |
NaotoMorita | 23:1509648c2318 | 279 | H(4,4) = 1.0f*(dcm(1,1)); |
NaotoMorita | 23:1509648c2318 | 280 | H(4,5) = 1.0f*(dcm(2,1)); |
NaotoMorita | 23:1509648c2318 | 281 | H(4,6) = 1.0f*(dcm(3,1)); |
NaotoMorita | 23:1509648c2318 | 282 | H(5,4) = 1.0f*(dcm(1,2)); |
NaotoMorita | 23:1509648c2318 | 283 | H(5,5) = 1.0f*(dcm(2,2)); |
NaotoMorita | 23:1509648c2318 | 284 | H(5,6) = 1.0f*(dcm(3,2)); |
NaotoMorita | 23:1509648c2318 | 285 | */ |
NaotoMorita | 23:1509648c2318 | 286 | Matrix nomVb = dcm*vihat; |
NaotoMorita | 23:1509648c2318 | 287 | Matrix qeterm = 2.0f*MatrixMath::Matrixcross(nomVb(1,1),nomVb(2,1),nomVb(3,1)); |
NaotoMorita | 23:1509648c2318 | 288 | for(int j = 1;j<4;j++){ |
NaotoMorita | 23:1509648c2318 | 289 | H(4,j) = -qeterm(1,j); |
NaotoMorita | 23:1509648c2318 | 290 | H(5,j) = -qeterm(3,j); |
NaotoMorita | 23:1509648c2318 | 291 | //H(3,j) = -qeterm(3,j); |
NaotoMorita | 23:1509648c2318 | 292 | H(4,9+j) = -dcm(1,j); |
NaotoMorita | 23:1509648c2318 | 293 | H(5,9+j) = -dcm(3,j); |
NaotoMorita | 23:1509648c2318 | 294 | //H(3,9+j) = -dcm(3,j); |
NaotoMorita | 23:1509648c2318 | 295 | } |
NaotoMorita | 23:1509648c2318 | 296 | |
NaotoMorita | 23:1509648c2318 | 297 | Matrix R(5,5); |
NaotoMorita | 23:1509648c2318 | 298 | R(1,1) = Ra(1,1)+Qab(1,1); |
NaotoMorita | 23:1509648c2318 | 299 | R(2,2) = Ra(2,2)+Qab(2,2); |
NaotoMorita | 23:1509648c2318 | 300 | R(3,3) = Ra(3,3)+Qab(3,3); |
NaotoMorita | 23:1509648c2318 | 301 | //R(4,4) = Rgsc(1,1); |
NaotoMorita | 23:1509648c2318 | 302 | //R(5,5) = Rgsc(2,2); |
NaotoMorita | 23:1509648c2318 | 303 | R(4,4) = Rvsc(1,1); |
NaotoMorita | 23:1509648c2318 | 304 | R(5,5) = Rvsc(2,2); |
NaotoMorita | 23:1509648c2318 | 305 | |
NaotoMorita | 23:1509648c2318 | 306 | Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R); |
NaotoMorita | 23:1509648c2318 | 307 | Matrix zacc = MatrixMath::Vector2mat(acc)-dcm*MatrixMath::Vector2mat(accref); |
NaotoMorita | 23:1509648c2318 | 308 | Matrix z(5,1); |
NaotoMorita | 23:1509648c2318 | 309 | z << zacc(1,1)<< zacc(2,1)<< zacc(3,1) << nomVb(1,1) << nomVb(3,1) ; |
NaotoMorita | 23:1509648c2318 | 310 | Matrix corrVal = K * (z-H*errState); |
NaotoMorita | 23:1509648c2318 | 311 | errState = errState + corrVal; |
NaotoMorita | 23:1509648c2318 | 312 | Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*(R)*MatrixMath::Transpose(K); |
NaotoMorita | 23:1509648c2318 | 313 | } |
NaotoMorita | 23:1509648c2318 | 314 | |
NaotoMorita | 23:1509648c2318 | 315 | |
NaotoMorita | 23:1509648c2318 | 316 | void ScErrStateEKF::updateStaticMeasures(Vector3 gyro,Vector3 acc, Vector3 accref) |
NaotoMorita | 23:1509648c2318 | 317 | { |
NaotoMorita | 23:1509648c2318 | 318 | acc -= accBias; |
NaotoMorita | 23:1509648c2318 | 319 | gyro -= gyroBias; |
NaotoMorita | 23:1509648c2318 | 320 | Matrix dcm(3,3); |
NaotoMorita | 23:1509648c2318 | 321 | computeDcm(dcm, qhat); |
NaotoMorita | 23:1509648c2318 | 322 | Vector3 gvec(dcm(1,3)*accref.z, dcm(2,3)*accref.z, dcm(3,3)*accref.z); |
NaotoMorita | 23:1509648c2318 | 323 | Matrix H(5,nState); |
NaotoMorita | 23:1509648c2318 | 324 | H(1,2) = -2.0f*gvec.z; |
NaotoMorita | 23:1509648c2318 | 325 | H(1,3) = 2.0f*gvec.y; |
NaotoMorita | 23:1509648c2318 | 326 | H(2,1) = 2.0f*gvec.z; |
NaotoMorita | 23:1509648c2318 | 327 | H(2,3) = -2.0f*gvec.x; |
NaotoMorita | 23:1509648c2318 | 328 | H(3,1) = -2.0f*gvec.y; |
NaotoMorita | 23:1509648c2318 | 329 | H(3,2) = 2.0f*gvec.x; |
NaotoMorita | 23:1509648c2318 | 330 | H(1,7) = 1.0f; |
NaotoMorita | 23:1509648c2318 | 331 | H(2,8) = 1.0f; |
NaotoMorita | 23:1509648c2318 | 332 | H(3,9) = 1.0f; |
NaotoMorita | 23:1509648c2318 | 333 | /* |
NaotoMorita | 23:1509648c2318 | 334 | Matrix qeterm = -2.0f*MatrixMath::Transpose(dcm)*MatrixMath::Matrixcross(gyro.x,gyro.y,gyro.z); |
NaotoMorita | 23:1509648c2318 | 335 | for(int j = 1;j<4;j++){ |
NaotoMorita | 23:1509648c2318 | 336 | H(4,j) = qeterm(1,j); |
NaotoMorita | 23:1509648c2318 | 337 | H(5,j) = qeterm(2,j); |
NaotoMorita | 23:1509648c2318 | 338 | } |
NaotoMorita | 23:1509648c2318 | 339 | H(4,4) = 1.0f*(dcm(1,1)); |
NaotoMorita | 23:1509648c2318 | 340 | H(4,5) = 1.0f*(dcm(2,1)); |
NaotoMorita | 23:1509648c2318 | 341 | H(4,6) = 1.0f*(dcm(3,1)); |
NaotoMorita | 23:1509648c2318 | 342 | H(5,4) = 1.0f*(dcm(1,2)); |
NaotoMorita | 23:1509648c2318 | 343 | H(5,5) = 1.0f*(dcm(2,2)); |
NaotoMorita | 23:1509648c2318 | 344 | H(5,6) = 1.0f*(dcm(3,2)); |
NaotoMorita | 23:1509648c2318 | 345 | */ |
NaotoMorita | 23:1509648c2318 | 346 | Matrix nomVb = dcm*vihat; |
NaotoMorita | 23:1509648c2318 | 347 | Matrix qeterm = 2.0f*MatrixMath::Matrixcross(nomVb(1,1),nomVb(2,1),nomVb(3,1)); |
NaotoMorita | 23:1509648c2318 | 348 | for(int j = 1;j<4;j++){ |
NaotoMorita | 23:1509648c2318 | 349 | H(4,j) = -qeterm(1,j); |
NaotoMorita | 23:1509648c2318 | 350 | H(5,j) = -qeterm(3,j); |
NaotoMorita | 23:1509648c2318 | 351 | //H(3,j) = -qeterm(3,j); |
NaotoMorita | 23:1509648c2318 | 352 | H(4,9+j) = -dcm(1,j); |
NaotoMorita | 23:1509648c2318 | 353 | H(5,9+j) = -dcm(3,j); |
NaotoMorita | 23:1509648c2318 | 354 | //H(3,9+j) = -dcm(3,j); |
NaotoMorita | 23:1509648c2318 | 355 | } |
NaotoMorita | 23:1509648c2318 | 356 | |
NaotoMorita | 23:1509648c2318 | 357 | Matrix R(5,5); |
NaotoMorita | 23:1509648c2318 | 358 | R(1,1) = Ra(1,1)*100.0f; |
NaotoMorita | 23:1509648c2318 | 359 | R(2,2) = Ra(2,2)*100.0f; |
NaotoMorita | 23:1509648c2318 | 360 | R(3,3) = Ra(3,3)*100.0f; |
NaotoMorita | 23:1509648c2318 | 361 | //R(4,4) = Rgsc(1,1); |
NaotoMorita | 23:1509648c2318 | 362 | //R(5,5) = Rgsc(2,2); |
NaotoMorita | 23:1509648c2318 | 363 | R(4,4) = Rvsc(1,1); |
NaotoMorita | 23:1509648c2318 | 364 | R(5,5) = Rvsc(2,2); |
NaotoMorita | 23:1509648c2318 | 365 | |
NaotoMorita | 23:1509648c2318 | 366 | Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R); |
NaotoMorita | 23:1509648c2318 | 367 | Matrix zacc = MatrixMath::Vector2mat(acc)-dcm*MatrixMath::Vector2mat(accref); |
NaotoMorita | 23:1509648c2318 | 368 | Matrix z(5,1); |
NaotoMorita | 23:1509648c2318 | 369 | z << zacc(1,1)<< zacc(2,1)<< zacc(3,1) << nomVb(1,1) << nomVb(3,1) ; |
NaotoMorita | 23:1509648c2318 | 370 | Matrix corrVal = K * (z-H*errState); |
NaotoMorita | 23:1509648c2318 | 371 | errState = errState + corrVal; |
NaotoMorita | 23:1509648c2318 | 372 | Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*(R)*MatrixMath::Transpose(K); |
NaotoMorita | 23:1509648c2318 | 373 | } |
NaotoMorita | 23:1509648c2318 | 374 | |
NaotoMorita | 23:1509648c2318 | 375 | void ScErrStateEKF::updateConstraints(Vector3 gyro) |
NaotoMorita | 23:1509648c2318 | 376 | { |
NaotoMorita | 23:1509648c2318 | 377 | |
NaotoMorita | 23:1509648c2318 | 378 | gyro -=gyroBias; |
NaotoMorita | 23:1509648c2318 | 379 | |
NaotoMorita | 23:1509648c2318 | 380 | Matrix dcm(3,3); |
NaotoMorita | 23:1509648c2318 | 381 | computeDcm(dcm, qhat); |
NaotoMorita | 23:1509648c2318 | 382 | |
NaotoMorita | 23:1509648c2318 | 383 | Matrix qeterm = -2.0f*MatrixMath::Transpose(dcm)*MatrixMath::Matrixcross(gyro.x,gyro.y,gyro.z); |
NaotoMorita | 23:1509648c2318 | 384 | Matrix H(4,nState); |
NaotoMorita | 21:d6079def0473 | 385 | for(int i = 1; i<3; i++){ |
NaotoMorita | 21:d6079def0473 | 386 | for(int j = 1;j<4;j++){ |
NaotoMorita | 21:d6079def0473 | 387 | H(i,j) = qeterm(i,j); |
NaotoMorita | 21:d6079def0473 | 388 | } |
NaotoMorita | 21:d6079def0473 | 389 | } |
NaotoMorita | 21:d6079def0473 | 390 | H(1,4) = 1.0f*(dcm(1,1)); |
NaotoMorita | 21:d6079def0473 | 391 | H(1,5) = 1.0f*(dcm(2,1)); |
NaotoMorita | 21:d6079def0473 | 392 | H(1,6) = 1.0f*(dcm(3,1)); |
NaotoMorita | 21:d6079def0473 | 393 | H(2,4) = 1.0f*(dcm(1,2)); |
NaotoMorita | 21:d6079def0473 | 394 | H(2,5) = 1.0f*(dcm(2,2)); |
NaotoMorita | 21:d6079def0473 | 395 | H(2,6) = 1.0f*(dcm(3,2)); |
NaotoMorita | 23:1509648c2318 | 396 | |
NaotoMorita | 23:1509648c2318 | 397 | Matrix nomVb = dcm*vihat; |
NaotoMorita | 23:1509648c2318 | 398 | qeterm = -2.0f*MatrixMath::Matrixcross(nomVb(1,1),nomVb(2,1),nomVb(3,1)); |
NaotoMorita | 23:1509648c2318 | 399 | for(int j = 1;j<4;j++){ |
NaotoMorita | 23:1509648c2318 | 400 | H(3,j) = -qeterm(1,j); |
NaotoMorita | 23:1509648c2318 | 401 | H(4,j) = -qeterm(3,j); |
NaotoMorita | 23:1509648c2318 | 402 | //H(3,j) = -qeterm(3,j); |
NaotoMorita | 23:1509648c2318 | 403 | H(3,9+j) = -dcm(1,j); |
NaotoMorita | 23:1509648c2318 | 404 | H(4,9+j) = -dcm(3,j); |
NaotoMorita | 23:1509648c2318 | 405 | //H(3,9+j) = -dcm(3,j); |
NaotoMorita | 23:1509648c2318 | 406 | } |
NaotoMorita | 23:1509648c2318 | 407 | |
NaotoMorita | 23:1509648c2318 | 408 | |
NaotoMorita | 23:1509648c2318 | 409 | Matrix R(4,4); |
NaotoMorita | 23:1509648c2318 | 410 | R(1,1) = Rgsc(1,1); |
NaotoMorita | 23:1509648c2318 | 411 | R(2,2) = Rgsc(2,1); |
NaotoMorita | 23:1509648c2318 | 412 | R(3,3) = Rvsc(1,1); |
NaotoMorita | 23:1509648c2318 | 413 | R(4,4) = Rvsc(2,1); |
NaotoMorita | 23:1509648c2318 | 414 | //R(5,5) = Rvsc(3,1); |
NaotoMorita | 23:1509648c2318 | 415 | Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R); |
NaotoMorita | 23:1509648c2318 | 416 | Matrix z(4,1); |
NaotoMorita | 23:1509648c2318 | 417 | 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 | 20:37d3c3ee36e9 | 418 | Matrix corrVal = K * (z-H*errState); |
NaotoMorita | 20:37d3c3ee36e9 | 419 | errState = errState + corrVal; |
NaotoMorita | 23:1509648c2318 | 420 | Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*(R)*MatrixMath::Transpose(K); |
NaotoMorita | 25:07ac5c6cd61c | 421 | } |
NaotoMorita | 25:07ac5c6cd61c | 422 | |
NaotoMorita | 21:d6079def0473 | 423 | |
NaotoMorita | 21:d6079def0473 | 424 | |
NaotoMorita | 19:3fae66745363 | 425 | void ScErrStateEKF::updateStaticAccMeasures(Vector3 acc, Vector3 accref) |
NaotoMorita | 19:3fae66745363 | 426 | { |
NaotoMorita | 19:3fae66745363 | 427 | Matrix dcm(3,3); |
NaotoMorita | 19:3fae66745363 | 428 | computeDcm(dcm, qhat); |
NaotoMorita | 19:3fae66745363 | 429 | Vector3 gvec(dcm(1,3)*accref.z, dcm(2,3)*accref.z, dcm(3,3)*accref.z); |
NaotoMorita | 21:d6079def0473 | 430 | Matrix H(3,nState); |
NaotoMorita | 19:3fae66745363 | 431 | H(1,2) = -2.0f*gvec.z; |
NaotoMorita | 19:3fae66745363 | 432 | H(1,3) = 2.0f*gvec.y; |
NaotoMorita | 19:3fae66745363 | 433 | H(2,1) = 2.0f*gvec.z; |
NaotoMorita | 19:3fae66745363 | 434 | H(2,3) = -2.0f*gvec.x; |
NaotoMorita | 19:3fae66745363 | 435 | H(3,1) = -2.0f*gvec.y; |
NaotoMorita | 19:3fae66745363 | 436 | H(3,2) = 2.0f*gvec.x; |
NaotoMorita | 19:3fae66745363 | 437 | Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+Ra*100.0f); |
NaotoMorita | 19:3fae66745363 | 438 | Matrix accmat(3,1); |
NaotoMorita | 19:3fae66745363 | 439 | accmat << acc.x << acc.y << acc.z; |
NaotoMorita | 19:3fae66745363 | 440 | Matrix gref(3,1); |
NaotoMorita | 19:3fae66745363 | 441 | gref << 0.0f << 0.0f << accref.z; |
NaotoMorita | 19:3fae66745363 | 442 | Matrix z = accmat-dcm*gref; |
NaotoMorita | 19:3fae66745363 | 443 | errState = errState + K * (z-H*errState); |
NaotoMorita | 21:d6079def0473 | 444 | Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*(Ra*100.0f)*MatrixMath::Transpose(K); |
NaotoMorita | 19:3fae66745363 | 445 | } |
NaotoMorita | 19:3fae66745363 | 446 | |
NaotoMorita | 19:3fae66745363 | 447 | void ScErrStateEKF::updateMagMeasures(Vector3 mag) |
NaotoMorita | 19:3fae66745363 | 448 | { |
NaotoMorita | 19:3fae66745363 | 449 | //mag = mag/mag.Norm(); |
NaotoMorita | 19:3fae66745363 | 450 | Matrix dcm(3,3); |
NaotoMorita | 19:3fae66745363 | 451 | computeDcm(dcm, qhat); |
NaotoMorita | 19:3fae66745363 | 452 | |
NaotoMorita | 19:3fae66745363 | 453 | Matrix magvec(3,1); |
NaotoMorita | 19:3fae66745363 | 454 | magvec(1,1) = mag.x; |
NaotoMorita | 19:3fae66745363 | 455 | magvec(2,1) = mag.y; |
NaotoMorita | 19:3fae66745363 | 456 | magvec(3,1) = mag.z; |
NaotoMorita | 19:3fae66745363 | 457 | |
NaotoMorita | 19:3fae66745363 | 458 | Matrix magnedvec = MatrixMath::Transpose(dcm)*magvec; |
NaotoMorita | 19:3fae66745363 | 459 | Matrix magrefmod(3,1); |
NaotoMorita | 19:3fae66745363 | 460 | magrefmod(1,1) = sqrt(magnedvec(1,1)*magnedvec(1,1)+magnedvec(2,1)*magnedvec(2,1)); |
NaotoMorita | 19:3fae66745363 | 461 | magrefmod(2,1) = 0.0f; |
NaotoMorita | 19:3fae66745363 | 462 | magrefmod(3,1) = magnedvec(3,1); |
NaotoMorita | 19:3fae66745363 | 463 | |
NaotoMorita | 19:3fae66745363 | 464 | Matrix mvec = dcm*magrefmod ; |
NaotoMorita | 21:d6079def0473 | 465 | Matrix H(3,nState); |
NaotoMorita | 19:3fae66745363 | 466 | H(1,2) = -2.0f*magvec(3,1); |
NaotoMorita | 19:3fae66745363 | 467 | H(1,3) = 2.0f*magvec(2,1); |
NaotoMorita | 19:3fae66745363 | 468 | H(2,1) = 2.0f*magvec(3,1); |
NaotoMorita | 19:3fae66745363 | 469 | H(2,3) = -2.0f*magvec(1,1); |
NaotoMorita | 19:3fae66745363 | 470 | H(3,1) = -2.0f*magvec(2,1); |
NaotoMorita | 19:3fae66745363 | 471 | H(3,2) = 2.0f*magvec(1,1); |
NaotoMorita | 21:d6079def0473 | 472 | Matrix Pm(nState,nState); |
NaotoMorita | 19:3fae66745363 | 473 | for(int i = 1; i<4; i++){ |
NaotoMorita | 19:3fae66745363 | 474 | for(int j = 1;j<4;j++){ |
NaotoMorita | 19:3fae66745363 | 475 | Pm(i,j) = Phat(i,j); |
NaotoMorita | 19:3fae66745363 | 476 | } |
NaotoMorita | 19:3fae66745363 | 477 | } |
NaotoMorita | 19:3fae66745363 | 478 | Matrix r3(3,1); |
NaotoMorita | 19:3fae66745363 | 479 | r3 << dcm(1,3)<< dcm(2,3) << dcm(3,3); |
NaotoMorita | 19:3fae66745363 | 480 | Matrix kpart = r3*MatrixMath::Transpose(r3); |
NaotoMorita | 21:d6079def0473 | 481 | Matrix Kmod(nState,nState); |
NaotoMorita | 19:3fae66745363 | 482 | for(int i = 1; i<4; i++){ |
NaotoMorita | 19:3fae66745363 | 483 | for(int j = 1;j<4;j++){ |
NaotoMorita | 19:3fae66745363 | 484 | Kmod(i,j) = kpart(i,j); |
NaotoMorita | 19:3fae66745363 | 485 | } |
NaotoMorita | 19:3fae66745363 | 486 | } |
NaotoMorita | 19:3fae66745363 | 487 | |
NaotoMorita | 19:3fae66745363 | 488 | Matrix K = Kmod*(Pm*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Pm*MatrixMath::Transpose(H)+Rm); |
NaotoMorita | 19:3fae66745363 | 489 | Matrix z = magvec-mvec; |
NaotoMorita | 19:3fae66745363 | 490 | errState = errState + K * (z-H*errState); |
NaotoMorita | 21:d6079def0473 | 491 | Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*(Rm)*MatrixMath::Transpose(K); |
NaotoMorita | 19:3fae66745363 | 492 | } |
NaotoMorita | 19:3fae66745363 | 493 | |
NaotoMorita | 25:07ac5c6cd61c | 494 | void ScErrStateEKF::updateCenteredGyroBiasCorrection(float (&cBg)[3]) |
NaotoMorita | 25:07ac5c6cd61c | 495 | { |
NaotoMorita | 25:07ac5c6cd61c | 496 | |
NaotoMorita | 25:07ac5c6cd61c | 497 | Matrix RgyroBias(3,3); |
NaotoMorita | 25:07ac5c6cd61c | 498 | /* |
NaotoMorita | 25:07ac5c6cd61c | 499 | RgyroBias(1,1) = cBgCov[0]; |
NaotoMorita | 25:07ac5c6cd61c | 500 | RgyroBias(1,2) = cBgCov[1]; |
NaotoMorita | 25:07ac5c6cd61c | 501 | RgyroBias(2,1) = cBgCov[1]; |
NaotoMorita | 25:07ac5c6cd61c | 502 | RgyroBias(1,3) = cBgCov[2]; |
NaotoMorita | 25:07ac5c6cd61c | 503 | RgyroBias(3,1) = cBgCov[2]; |
NaotoMorita | 25:07ac5c6cd61c | 504 | RgyroBias(2,2) = cBgCov[3]; |
NaotoMorita | 25:07ac5c6cd61c | 505 | RgyroBias(2,3) = cBgCov[4]; |
NaotoMorita | 25:07ac5c6cd61c | 506 | RgyroBias(3,2) = cBgCov[4]; |
NaotoMorita | 25:07ac5c6cd61c | 507 | RgyroBias(3,3) = cBgCov[5]; |
NaotoMorita | 25:07ac5c6cd61c | 508 | */ |
NaotoMorita | 25:07ac5c6cd61c | 509 | for(int i = 1;i<4;i++){ |
NaotoMorita | 25:07ac5c6cd61c | 510 | RgyroBias(i,i) = 0.1f; |
NaotoMorita | 25:07ac5c6cd61c | 511 | } |
NaotoMorita | 25:07ac5c6cd61c | 512 | |
NaotoMorita | 25:07ac5c6cd61c | 513 | Matrix H(3,nState); |
NaotoMorita | 25:07ac5c6cd61c | 514 | H(1,4) = 1.0f; |
NaotoMorita | 25:07ac5c6cd61c | 515 | H(2,5) = 1.0f; |
NaotoMorita | 25:07ac5c6cd61c | 516 | H(3,6) = 1.0f; |
NaotoMorita | 25:07ac5c6cd61c | 517 | |
NaotoMorita | 25:07ac5c6cd61c | 518 | Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+RgyroBias); |
NaotoMorita | 25:07ac5c6cd61c | 519 | Matrix z(3,1); |
NaotoMorita | 25:07ac5c6cd61c | 520 | z <<cBg[0]-gyroBias.x <<cBg[1]-gyroBias.y <<cBg[2]-gyroBias.z; |
NaotoMorita | 25:07ac5c6cd61c | 521 | Matrix corrVal = K * (z-H*errState); |
NaotoMorita | 25:07ac5c6cd61c | 522 | errState = errState + corrVal; |
NaotoMorita | 25:07ac5c6cd61c | 523 | Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*(RgyroBias)*MatrixMath::Transpose(K); |
NaotoMorita | 25:07ac5c6cd61c | 524 | |
NaotoMorita | 25:07ac5c6cd61c | 525 | //gyroBias.x = cBg[0]; |
NaotoMorita | 25:07ac5c6cd61c | 526 | //gyroBias.y = cBg[1]; |
NaotoMorita | 25:07ac5c6cd61c | 527 | //gyroBias.z = cBg[2]; |
NaotoMorita | 25:07ac5c6cd61c | 528 | } |
NaotoMorita | 25:07ac5c6cd61c | 529 | |
NaotoMorita | 25:07ac5c6cd61c | 530 | void ScErrStateEKF::updateCenteredAccBiasCorrection(float (&cBa)[3]) |
NaotoMorita | 25:07ac5c6cd61c | 531 | { |
NaotoMorita | 25:07ac5c6cd61c | 532 | |
NaotoMorita | 25:07ac5c6cd61c | 533 | Matrix RaccBias(3,3); |
NaotoMorita | 25:07ac5c6cd61c | 534 | for(int i = 1;i<4;i++){ |
NaotoMorita | 25:07ac5c6cd61c | 535 | RaccBias(i,i) = 5.0f; |
NaotoMorita | 25:07ac5c6cd61c | 536 | } |
NaotoMorita | 25:07ac5c6cd61c | 537 | |
NaotoMorita | 25:07ac5c6cd61c | 538 | Matrix H(3,nState); |
NaotoMorita | 25:07ac5c6cd61c | 539 | H(1,7) = 1.0f; |
NaotoMorita | 25:07ac5c6cd61c | 540 | H(2,8) = 1.0f; |
NaotoMorita | 25:07ac5c6cd61c | 541 | H(3,9) = 1.0f; |
NaotoMorita | 25:07ac5c6cd61c | 542 | |
NaotoMorita | 25:07ac5c6cd61c | 543 | Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+RaccBias); |
NaotoMorita | 25:07ac5c6cd61c | 544 | Matrix z(3,1); |
NaotoMorita | 25:07ac5c6cd61c | 545 | z <<cBa[0]-accBias.x <<cBa[1]-accBias.y <<cBa[2]-accBias.z; |
NaotoMorita | 25:07ac5c6cd61c | 546 | Matrix corrVal = K * (z-H*errState); |
NaotoMorita | 25:07ac5c6cd61c | 547 | errState = errState + corrVal; |
NaotoMorita | 25:07ac5c6cd61c | 548 | Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*(RaccBias)*MatrixMath::Transpose(K); |
NaotoMorita | 25:07ac5c6cd61c | 549 | |
NaotoMorita | 25:07ac5c6cd61c | 550 | //gyroBias.x = cBg[0]; |
NaotoMorita | 25:07ac5c6cd61c | 551 | //gyroBias.y = cBg[1]; |
NaotoMorita | 25:07ac5c6cd61c | 552 | //gyroBias.z = cBg[2]; |
NaotoMorita | 25:07ac5c6cd61c | 553 | } |
NaotoMorita | 25:07ac5c6cd61c | 554 | |
NaotoMorita | 25:07ac5c6cd61c | 555 | |
NaotoMorita | 19:3fae66745363 | 556 | void ScErrStateEKF::resetBias() |
NaotoMorita | 19:3fae66745363 | 557 | { |
NaotoMorita | 19:3fae66745363 | 558 | gyroBias.x = gyroBias.x + errState(4,1)*1.0f; |
NaotoMorita | 19:3fae66745363 | 559 | gyroBias.y = gyroBias.y + errState(5,1)*1.0f; |
NaotoMorita | 19:3fae66745363 | 560 | gyroBias.z = gyroBias.z + errState(6,1)*1.0f; |
NaotoMorita | 19:3fae66745363 | 561 | errState(4,1) = 0.0f; |
NaotoMorita | 19:3fae66745363 | 562 | errState(5,1) = 0.0f; |
NaotoMorita | 19:3fae66745363 | 563 | errState(6,1) = 0.0f; |
NaotoMorita | 20:37d3c3ee36e9 | 564 | accBias.x = accBias.x + errState(7,1); |
NaotoMorita | 20:37d3c3ee36e9 | 565 | accBias.y = accBias.y + errState(8,1); |
NaotoMorita | 20:37d3c3ee36e9 | 566 | accBias.z = accBias.z + errState(9,1); |
NaotoMorita | 20:37d3c3ee36e9 | 567 | errState(7,1) = 0.0f; |
NaotoMorita | 20:37d3c3ee36e9 | 568 | errState(8,1) = 0.0f; |
NaotoMorita | 20:37d3c3ee36e9 | 569 | errState(9,1) = 0.0f; |
NaotoMorita | 19:3fae66745363 | 570 | } |
NaotoMorita | 19:3fae66745363 | 571 | |
NaotoMorita | 19:3fae66745363 | 572 | void ScErrStateEKF::computeAngles(Vector3& rpy,Vector3 rpy_align) |
NaotoMorita | 19:3fae66745363 | 573 | { |
NaotoMorita | 19:3fae66745363 | 574 | Matrix qerr(4,1); |
NaotoMorita | 19:3fae66745363 | 575 | qerr << 1.0f << errState(1,1) << errState(2,1) << errState(3,1); |
NaotoMorita | 19:3fae66745363 | 576 | //float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qerr),qerr)); |
NaotoMorita | 19:3fae66745363 | 577 | //qerr *= (1.0f/ qnorm); |
NaotoMorita | 19:3fae66745363 | 578 | Matrix qest = quatmultiply(qhat, qerr); |
NaotoMorita | 19:3fae66745363 | 579 | float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qest),qest)); |
NaotoMorita | 19:3fae66745363 | 580 | qest *= (1.0f/ qnorm); |
NaotoMorita | 19:3fae66745363 | 581 | float q0 = qest( 1, 1 ); |
NaotoMorita | 19:3fae66745363 | 582 | float q1 = qest( 2, 1 ); |
NaotoMorita | 19:3fae66745363 | 583 | float q2 = qest( 3, 1 ); |
NaotoMorita | 19:3fae66745363 | 584 | float q3 = qest( 4, 1 ); |
NaotoMorita | 19:3fae66745363 | 585 | rpy.x = atan2f(q0*q1 + q2*q3, 0.5f - q1*q1 - q2*q2)-rpy_align.x; |
NaotoMorita | 19:3fae66745363 | 586 | rpy.y = asinf(-2.0f * (q1*q3 - q0*q2))-rpy_align.y; |
NaotoMorita | 19:3fae66745363 | 587 | rpy.z = atan2f(q1*q2 + q0*q3, 0.5f - q2*q2 - q3*q3); |
NaotoMorita | 19:3fae66745363 | 588 | |
NaotoMorita | 19:3fae66745363 | 589 | } |
NaotoMorita | 19:3fae66745363 | 590 | |
NaotoMorita | 21:d6079def0473 | 591 | |
NaotoMorita | 21:d6079def0473 | 592 | void ScErrStateEKF::fuseErr2Nominal() |
NaotoMorita | 19:3fae66745363 | 593 | { |
NaotoMorita | 19:3fae66745363 | 594 | Matrix qerr(4,1); |
NaotoMorita | 19:3fae66745363 | 595 | qerr << 1.0f << errState(1,1) << errState(2,1) << errState(3,1); |
NaotoMorita | 19:3fae66745363 | 596 | //float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qerr),qerr)); |
NaotoMorita | 19:3fae66745363 | 597 | //qerr *= (1.0f/ qnorm); |
NaotoMorita | 19:3fae66745363 | 598 | qhat = quatmultiply(qhat, qerr); |
NaotoMorita | 19:3fae66745363 | 599 | errState(1,1) = 0.0f; |
NaotoMorita | 19:3fae66745363 | 600 | errState(2,1) = 0.0f; |
NaotoMorita | 19:3fae66745363 | 601 | errState(3,1) = 0.0f; |
NaotoMorita | 19:3fae66745363 | 602 | float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qhat),qhat)); |
NaotoMorita | 19:3fae66745363 | 603 | qhat *= (1.0f/ qnorm); |
NaotoMorita | 23:1509648c2318 | 604 | |
NaotoMorita | 23:1509648c2318 | 605 | vihat(1,1) += errState(10,1); |
NaotoMorita | 23:1509648c2318 | 606 | vihat(2,1) += errState(11,1); |
NaotoMorita | 23:1509648c2318 | 607 | vihat(3,1) += errState(12,1); |
NaotoMorita | 23:1509648c2318 | 608 | errState(10,1) = 0.0f; |
NaotoMorita | 23:1509648c2318 | 609 | errState(11,1) = 0.0f; |
NaotoMorita | 23:1509648c2318 | 610 | errState(12,1) = 0.0f; |
NaotoMorita | 19:3fae66745363 | 611 | } |
NaotoMorita | 19:3fae66745363 | 612 | |
NaotoMorita | 19:3fae66745363 | 613 | Matrix ScErrStateEKF::quatmultiply(Matrix q, Matrix r) |
NaotoMorita | 19:3fae66745363 | 614 | { |
NaotoMorita | 19:3fae66745363 | 615 | Matrix qout(4,1); |
NaotoMorita | 19:3fae66745363 | 616 | 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 | 617 | 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 | 618 | 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 | 619 | 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 | 620 | float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qout),qout)); |
NaotoMorita | 19:3fae66745363 | 621 | qout *= (1.0f/ qnorm); |
NaotoMorita | 19:3fae66745363 | 622 | return qout; |
NaotoMorita | 19:3fae66745363 | 623 | } |
NaotoMorita | 19:3fae66745363 | 624 | |
NaotoMorita | 19:3fae66745363 | 625 | void ScErrStateEKF::computeDcm(Matrix& dcm, Matrix quat) |
NaotoMorita | 19:3fae66745363 | 626 | { |
NaotoMorita | 19:3fae66745363 | 627 | |
NaotoMorita | 19:3fae66745363 | 628 | float q0 = quat( 1, 1 ); |
NaotoMorita | 19:3fae66745363 | 629 | float q1 = quat( 2, 1 ); |
NaotoMorita | 19:3fae66745363 | 630 | float q2 = quat( 3, 1 ); |
NaotoMorita | 19:3fae66745363 | 631 | float q3 = quat( 4, 1 ); |
NaotoMorita | 19:3fae66745363 | 632 | |
NaotoMorita | 19:3fae66745363 | 633 | dcm(1,1) = q0*q0 + q1*q1 - q2*q2 - q3*q3; |
NaotoMorita | 19:3fae66745363 | 634 | dcm(1,2) = 2*(q1*q2 + q0*q3); |
NaotoMorita | 19:3fae66745363 | 635 | dcm(1,3) = 2*(q1*q3 - q0*q2); |
NaotoMorita | 19:3fae66745363 | 636 | dcm(2,1) = 2*(q1*q2 - q0*q3); |
NaotoMorita | 19:3fae66745363 | 637 | dcm(2,2) = q0*q0 - q1*q1 + q2*q2 - q3*q3; |
NaotoMorita | 19:3fae66745363 | 638 | dcm(2,3) = 2*(q2*q3 + q0*q1); |
NaotoMorita | 19:3fae66745363 | 639 | dcm(3,1) = 2*(q1*q3 + q0*q2); |
NaotoMorita | 19:3fae66745363 | 640 | dcm(3,2) = 2*(q2*q3 - q0*q1); |
NaotoMorita | 19:3fae66745363 | 641 | dcm(3,3) = q0*q0 - q1*q1 - q2*q2 + q3*q3; |
NaotoMorita | 19:3fae66745363 | 642 | |
NaotoMorita | 19:3fae66745363 | 643 | } |
NaotoMorita | 19:3fae66745363 | 644 | |
NaotoMorita | 19:3fae66745363 | 645 | void ScErrStateEKF::defineQhat(Vector3 align){ |
NaotoMorita | 19:3fae66745363 | 646 | float cos_z_2 = cosf(0.5f*align.z); |
NaotoMorita | 19:3fae66745363 | 647 | float cos_y_2 = cosf(0.5f*align.y); |
NaotoMorita | 19:3fae66745363 | 648 | float cos_x_2 = cosf(0.5f*align.x); |
NaotoMorita | 19:3fae66745363 | 649 | |
NaotoMorita | 19:3fae66745363 | 650 | float sin_z_2 = sinf(0.5f*align.z); |
NaotoMorita | 19:3fae66745363 | 651 | float sin_y_2 = sinf(0.5f*align.y); |
NaotoMorita | 19:3fae66745363 | 652 | float sin_x_2 = sinf(0.5f*align.x); |
NaotoMorita | 19:3fae66745363 | 653 | |
NaotoMorita | 19:3fae66745363 | 654 | // and now compute quaternion |
NaotoMorita | 19:3fae66745363 | 655 | qhat(1,1) = cos_z_2*cos_y_2*cos_x_2 + sin_z_2*sin_y_2*sin_x_2; |
NaotoMorita | 19:3fae66745363 | 656 | qhat(2,1) = cos_z_2*cos_y_2*sin_x_2 - sin_z_2*sin_y_2*cos_x_2; |
NaotoMorita | 19:3fae66745363 | 657 | qhat(3,1) = cos_z_2*sin_y_2*cos_x_2 + sin_z_2*cos_y_2*sin_x_2; |
NaotoMorita | 19:3fae66745363 | 658 | qhat(4,1) = sin_z_2*cos_y_2*cos_x_2 - cos_z_2*sin_y_2*sin_x_2; |
NaotoMorita | 19:3fae66745363 | 659 | } |
NaotoMorita | 19:3fae66745363 | 660 | |
NaotoMorita | 23:1509648c2318 | 661 | void ScErrStateEKF::computeVb(Vector3& vb) |
NaotoMorita | 23:1509648c2318 | 662 | { |
NaotoMorita | 23:1509648c2318 | 663 | Matrix dcm(3,3); |
NaotoMorita | 23:1509648c2318 | 664 | computeDcm(dcm, qhat); |
NaotoMorita | 23:1509648c2318 | 665 | Matrix vbmat = dcm*vihat; |
NaotoMorita | 23:1509648c2318 | 666 | vb.x = vbmat(1,1); |
NaotoMorita | 23:1509648c2318 | 667 | vb.y = vbmat(2,1); |
NaotoMorita | 23:1509648c2318 | 668 | vb.z = vbmat(3,1); |
NaotoMorita | 23:1509648c2318 | 669 | |
NaotoMorita | 23:1509648c2318 | 670 | } |
NaotoMorita | 23:1509648c2318 | 671 | |
NaotoMorita | 19:3fae66745363 | 672 | |
NaotoMorita | 19:3fae66745363 | 673 | |
NaotoMorita | 19:3fae66745363 | 674 | Vector3 ScErrStateEKF::calcDynAcc(Vector3 acc, Vector3 accref) |
NaotoMorita | 19:3fae66745363 | 675 | { |
NaotoMorita | 19:3fae66745363 | 676 | Matrix dcm(3,3); |
NaotoMorita | 19:3fae66745363 | 677 | computeDcm(dcm, qhat); |
NaotoMorita | 19:3fae66745363 | 678 | float _x, _y, _z; |
NaotoMorita | 19:3fae66745363 | 679 | _x = acc.x-(dcm( 1, 1 )*accref.x+dcm( 1, 2 )*accref.y+dcm( 1, 3 )*accref.z); |
NaotoMorita | 19:3fae66745363 | 680 | _y = acc.y-(dcm( 2, 1 )*accref.x+dcm( 2, 2 )*accref.y+dcm( 2, 3 )*accref.z); |
NaotoMorita | 19:3fae66745363 | 681 | _z = acc.z-(dcm( 3, 1 )*accref.x+dcm( 3, 2 )*accref.y+dcm( 3, 3 )*accref.z); |
NaotoMorita | 19:3fae66745363 | 682 | return Vector3(_x, _y, _z); |
NaotoMorita | 19:3fae66745363 | 683 | } |
NaotoMorita | 19:3fae66745363 | 684 | |
NaotoMorita | 19:3fae66745363 | 685 | |
NaotoMorita | 19:3fae66745363 | 686 | bool ScErrStateEKF::determinDynStatus(Vector3 acc, Vector3 accref) |
NaotoMorita | 19:3fae66745363 | 687 | { |
NaotoMorita | 19:3fae66745363 | 688 | histffunc[histffuncindex] = acc.Norm()*acc.Norm()-accref.Norm()*accref.Norm()-3.0f*sigma2a; |
NaotoMorita | 19:3fae66745363 | 689 | if(histffuncindex<9){ |
NaotoMorita | 19:3fae66745363 | 690 | histffuncindex += 1; |
NaotoMorita | 19:3fae66745363 | 691 | }else{ |
NaotoMorita | 19:3fae66745363 | 692 | histffuncindex =0; |
NaotoMorita | 19:3fae66745363 | 693 | } |
NaotoMorita | 19:3fae66745363 | 694 | aveffunc = 0; |
NaotoMorita | 19:3fae66745363 | 695 | for(int i = 1;i<10;i++){ |
NaotoMorita | 19:3fae66745363 | 696 | aveffunc += 1.0f/10.0f*histffunc[i]; |
NaotoMorita | 19:3fae66745363 | 697 | } |
NaotoMorita | 19:3fae66745363 | 698 | sigma2f = 1.0f/10.0f*(6.0f*sigma2a*sigma2a+4.0f*accref.Norm()*accref.Norm()*sigma2a); |
NaotoMorita | 19:3fae66745363 | 699 | |
NaotoMorita | 19:3fae66745363 | 700 | |
NaotoMorita | 19:3fae66745363 | 701 | dynacc = calcDynAcc(acc,accref); |
NaotoMorita | 19:3fae66745363 | 702 | bool dynCase = true; |
NaotoMorita | 19:3fae66745363 | 703 | if((dynacc.Norm()<0.005f) && (abs(acc.Norm()-accref.Norm())<0.005f)){dynCase = false;} |
NaotoMorita | 19:3fae66745363 | 704 | if(aveffunc<sqrt(sigma2f/0.99f) && abs(acc.Norm()-accref.Norm())<0.001f){dynCase = false;} |
NaotoMorita | 19:3fae66745363 | 705 | return dynCase; |
NaotoMorita | 19:3fae66745363 | 706 | |
NaotoMorita | 19:3fae66745363 | 707 | } |
NaotoMorita | 19:3fae66745363 | 708 | |
NaotoMorita | 22:7d84b8bc20b4 | 709 | /* |
NaotoMorita | 22:7d84b8bc20b4 | 710 | void ScErrStateEKF::updateNominal(Vector3 gyro, Vector3 acc, Vector3 accref, float att_dt) |
NaotoMorita | 22:7d84b8bc20b4 | 711 | { |
NaotoMorita | 22:7d84b8bc20b4 | 712 | gyro -= gyroBias; |
NaotoMorita | 22:7d84b8bc20b4 | 713 | acc -= accBias; |
NaotoMorita | 22:7d84b8bc20b4 | 714 | Matrix A(4,4); |
NaotoMorita | 22:7d84b8bc20b4 | 715 | A << 0.0f << -0.5f*gyro.x <<-0.5f*gyro.y <<-0.5f*gyro.z |
NaotoMorita | 22:7d84b8bc20b4 | 716 | << 0.5f*gyro.x << 0.0f << 0.5f*gyro.z <<-0.5f*gyro.y |
NaotoMorita | 22:7d84b8bc20b4 | 717 | << 0.5f*gyro.y << -0.5f*gyro.z << 0.0f << 0.5f*gyro.x |
NaotoMorita | 22:7d84b8bc20b4 | 718 | << 0.5f*gyro.z << 0.5f*gyro.y <<-0.5f*gyro.x << 0.0f ; |
NaotoMorita | 22:7d84b8bc20b4 | 719 | |
NaotoMorita | 22:7d84b8bc20b4 | 720 | Matrix phi = MatrixMath::Eye(4) + A * att_dt + (0.5f*att_dt*att_dt) * A * A; |
NaotoMorita | 22:7d84b8bc20b4 | 721 | qhat = phi * qhat; |
NaotoMorita | 22:7d84b8bc20b4 | 722 | float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qhat),qhat)); |
NaotoMorita | 22:7d84b8bc20b4 | 723 | qhat *= (1.0f/ qnorm); |
NaotoMorita | 22:7d84b8bc20b4 | 724 | |
NaotoMorita | 22:7d84b8bc20b4 | 725 | Matrix dcm(3,3); |
NaotoMorita | 22:7d84b8bc20b4 | 726 | computeDcm(dcm, qhat); |
NaotoMorita | 22:7d84b8bc20b4 | 727 | vihat += (MatrixMath::Transpose(dcm)*MatrixMath::Vector2mat(acc)-MatrixMath::Vector2mat(accref))*att_dt*9.8f; |
NaotoMorita | 22:7d84b8bc20b4 | 728 | } |
NaotoMorita | 19:3fae66745363 | 729 | |
NaotoMorita | 22:7d84b8bc20b4 | 730 | void ScErrStateEKF::updateErrState(Vector3 gyro,Vector3 acc, float att_dt) |
NaotoMorita | 22:7d84b8bc20b4 | 731 | { |
NaotoMorita | 22:7d84b8bc20b4 | 732 | gyro -= gyroBias; |
NaotoMorita | 22:7d84b8bc20b4 | 733 | acc -= accBias; |
NaotoMorita | 22:7d84b8bc20b4 | 734 | Matrix A(nState,nState); |
NaotoMorita | 22:7d84b8bc20b4 | 735 | A(1,2) = gyro.z; |
NaotoMorita | 22:7d84b8bc20b4 | 736 | A(1,3) = -gyro.y; |
NaotoMorita | 22:7d84b8bc20b4 | 737 | A(2,1) = -gyro.z; |
NaotoMorita | 22:7d84b8bc20b4 | 738 | A(2,3) = gyro.x; |
NaotoMorita | 22:7d84b8bc20b4 | 739 | A(3,1) = gyro.y; |
NaotoMorita | 22:7d84b8bc20b4 | 740 | A(3,2) = -gyro.x; |
NaotoMorita | 22:7d84b8bc20b4 | 741 | A(1,4) = -0.5f; |
NaotoMorita | 22:7d84b8bc20b4 | 742 | A(2,5) = -0.5f; |
NaotoMorita | 22:7d84b8bc20b4 | 743 | A(3,6) = -0.5f; |
NaotoMorita | 22:7d84b8bc20b4 | 744 | |
NaotoMorita | 22:7d84b8bc20b4 | 745 | Matrix dcm(3,3); |
NaotoMorita | 22:7d84b8bc20b4 | 746 | computeDcm(dcm, qhat); |
NaotoMorita | 22:7d84b8bc20b4 | 747 | Matrix qeterm = -2.0f*9.8f*MatrixMath::Transpose(dcm)*MatrixMath::Matrixcross(acc.x,acc.y,acc.z); |
NaotoMorita | 22:7d84b8bc20b4 | 748 | Matrix baterm = -9.8f*MatrixMath::Transpose(dcm); |
NaotoMorita | 22:7d84b8bc20b4 | 749 | for (int i = 1; i < 4; i++){ |
NaotoMorita | 22:7d84b8bc20b4 | 750 | for (int j = 1; i < 4; i++){ |
NaotoMorita | 22:7d84b8bc20b4 | 751 | A(i+9,j) = qeterm(i,j); |
NaotoMorita | 22:7d84b8bc20b4 | 752 | A(i+9,j+6) = baterm(i,j); |
NaotoMorita | 22:7d84b8bc20b4 | 753 | } |
NaotoMorita | 22:7d84b8bc20b4 | 754 | } |
NaotoMorita | 22:7d84b8bc20b4 | 755 | |
NaotoMorita | 22:7d84b8bc20b4 | 756 | Matrix phi = MatrixMath::Eye(nState) + A * att_dt + (0.5f*att_dt*att_dt) * A * A; |
NaotoMorita | 22:7d84b8bc20b4 | 757 | 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 | 758 | errState = phi * errState; |
NaotoMorita | 22:7d84b8bc20b4 | 759 | Phat = phi*Phat*MatrixMath::Transpose(phi)+Qd; |
NaotoMorita | 22:7d84b8bc20b4 | 760 | } |
NaotoMorita | 22:7d84b8bc20b4 | 761 | |
NaotoMorita | 22:7d84b8bc20b4 | 762 | void ScErrStateEKF::updateVelocityConstraints() |
NaotoMorita | 22:7d84b8bc20b4 | 763 | { |
NaotoMorita | 22:7d84b8bc20b4 | 764 | |
NaotoMorita | 22:7d84b8bc20b4 | 765 | Matrix dcm(3,3); |
NaotoMorita | 22:7d84b8bc20b4 | 766 | computeDcm(dcm, qhat); |
NaotoMorita | 22:7d84b8bc20b4 | 767 | Matrix H(2,nState); |
NaotoMorita | 22:7d84b8bc20b4 | 768 | |
NaotoMorita | 22:7d84b8bc20b4 | 769 | Matrix nomVb = dcm*vihat; |
NaotoMorita | 22:7d84b8bc20b4 | 770 | Matrix qeterm = -2.0f*MatrixMath::Matrixcross(nomVb(1,1),nomVb(2,1),nomVb(3,1)); |
NaotoMorita | 22:7d84b8bc20b4 | 771 | for(int j = 1;j<4;j++){ |
NaotoMorita | 22:7d84b8bc20b4 | 772 | H(1,j) = -qeterm(1,j); |
NaotoMorita | 22:7d84b8bc20b4 | 773 | H(2,j) = -qeterm(3,j); |
NaotoMorita | 22:7d84b8bc20b4 | 774 | //H(3,j) = -qeterm(3,j); |
NaotoMorita | 22:7d84b8bc20b4 | 775 | H(1,9+j) = -dcm(1,j); |
NaotoMorita | 22:7d84b8bc20b4 | 776 | H(2,9+j) = -dcm(3,j); |
NaotoMorita | 22:7d84b8bc20b4 | 777 | //H(3,9+j) = -dcm(3,j); |
NaotoMorita | 22:7d84b8bc20b4 | 778 | } |
NaotoMorita | 22:7d84b8bc20b4 | 779 | |
NaotoMorita | 22:7d84b8bc20b4 | 780 | Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+Rvsc); |
NaotoMorita | 22:7d84b8bc20b4 | 781 | Matrix z(2,1); |
NaotoMorita | 22:7d84b8bc20b4 | 782 | z << nomVb(1,1) << nomVb(3,1) ; |
NaotoMorita | 22:7d84b8bc20b4 | 783 | Matrix corrVal = K * (z-H*errState); |
NaotoMorita | 22:7d84b8bc20b4 | 784 | errState = errState + corrVal; |
NaotoMorita | 22:7d84b8bc20b4 | 785 | Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*(Rvsc)*MatrixMath::Transpose(K); |
NaotoMorita | 22:7d84b8bc20b4 | 786 | |
NaotoMorita | 22:7d84b8bc20b4 | 787 | } |
NaotoMorita | 22:7d84b8bc20b4 | 788 | |
NaotoMorita | 22:7d84b8bc20b4 | 789 | void ScErrStateEKF::updateConstraints(Vector3 gyro) |
NaotoMorita | 22:7d84b8bc20b4 | 790 | { |
NaotoMorita | 22:7d84b8bc20b4 | 791 | |
NaotoMorita | 22:7d84b8bc20b4 | 792 | gyro -=gyroBias; |
NaotoMorita | 22:7d84b8bc20b4 | 793 | |
NaotoMorita | 22:7d84b8bc20b4 | 794 | Matrix dcm(3,3); |
NaotoMorita | 22:7d84b8bc20b4 | 795 | computeDcm(dcm, qhat); |
NaotoMorita | 22:7d84b8bc20b4 | 796 | |
NaotoMorita | 22:7d84b8bc20b4 | 797 | Matrix qeterm = -2.0f*MatrixMath::Transpose(dcm)*MatrixMath::Matrixcross(gyro.x,gyro.y,gyro.z); |
NaotoMorita | 22:7d84b8bc20b4 | 798 | Matrix H(4,nState); |
NaotoMorita | 22:7d84b8bc20b4 | 799 | for(int i = 1; i<3; i++){ |
NaotoMorita | 22:7d84b8bc20b4 | 800 | for(int j = 1;j<4;j++){ |
NaotoMorita | 22:7d84b8bc20b4 | 801 | H(i,j) = qeterm(i,j); |
NaotoMorita | 22:7d84b8bc20b4 | 802 | } |
NaotoMorita | 22:7d84b8bc20b4 | 803 | } |
NaotoMorita | 22:7d84b8bc20b4 | 804 | H(1,4) = 1.0f*(dcm(1,1)); |
NaotoMorita | 22:7d84b8bc20b4 | 805 | H(1,5) = 1.0f*(dcm(2,1)); |
NaotoMorita | 22:7d84b8bc20b4 | 806 | H(1,6) = 1.0f*(dcm(3,1)); |
NaotoMorita | 22:7d84b8bc20b4 | 807 | H(2,4) = 1.0f*(dcm(1,2)); |
NaotoMorita | 22:7d84b8bc20b4 | 808 | H(2,5) = 1.0f*(dcm(2,2)); |
NaotoMorita | 22:7d84b8bc20b4 | 809 | H(2,6) = 1.0f*(dcm(3,2)); |
NaotoMorita | 22:7d84b8bc20b4 | 810 | |
NaotoMorita | 22:7d84b8bc20b4 | 811 | Matrix nomVb = dcm*vihat; |
NaotoMorita | 22:7d84b8bc20b4 | 812 | qeterm = -2.0f*MatrixMath::Matrixcross(nomVb(1,1),nomVb(2,1),nomVb(3,1)); |
NaotoMorita | 22:7d84b8bc20b4 | 813 | for(int j = 1;j<4;j++){ |
NaotoMorita | 22:7d84b8bc20b4 | 814 | H(3,j) = -qeterm(1,j); |
NaotoMorita | 22:7d84b8bc20b4 | 815 | H(4,j) = -qeterm(3,j); |
NaotoMorita | 22:7d84b8bc20b4 | 816 | //H(3,j) = -qeterm(3,j); |
NaotoMorita | 22:7d84b8bc20b4 | 817 | H(3,9+j) = -dcm(1,j); |
NaotoMorita | 22:7d84b8bc20b4 | 818 | H(4,9+j) = -dcm(3,j); |
NaotoMorita | 22:7d84b8bc20b4 | 819 | //H(3,9+j) = -dcm(3,j); |
NaotoMorita | 22:7d84b8bc20b4 | 820 | } |
NaotoMorita | 22:7d84b8bc20b4 | 821 | |
NaotoMorita | 22:7d84b8bc20b4 | 822 | Matrix R(4,4); |
NaotoMorita | 22:7d84b8bc20b4 | 823 | R(1,1) = Rgsc(1,1); |
NaotoMorita | 22:7d84b8bc20b4 | 824 | R(2,2) = Rgsc(2,1); |
NaotoMorita | 22:7d84b8bc20b4 | 825 | R(3,3) = Rvsc(1,1); |
NaotoMorita | 22:7d84b8bc20b4 | 826 | R(4,4) = Rvsc(2,1); |
NaotoMorita | 22:7d84b8bc20b4 | 827 | //R(5,5) = Rvsc(3,1); |
NaotoMorita | 22:7d84b8bc20b4 | 828 | Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R); |
NaotoMorita | 22:7d84b8bc20b4 | 829 | Matrix z(4,1); |
NaotoMorita | 22:7d84b8bc20b4 | 830 | 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 | 831 | Matrix corrVal = K * (z-H*errState); |
NaotoMorita | 22:7d84b8bc20b4 | 832 | errState = errState + corrVal; |
NaotoMorita | 22:7d84b8bc20b4 | 833 | Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*(R)*MatrixMath::Transpose(K); |
NaotoMorita | 22:7d84b8bc20b4 | 834 | } |
NaotoMorita | 22:7d84b8bc20b4 | 835 | |
NaotoMorita | 22:7d84b8bc20b4 | 836 | void ScErrStateEKF::computeVb(Vector3& vb) |
NaotoMorita | 22:7d84b8bc20b4 | 837 | { |
NaotoMorita | 22:7d84b8bc20b4 | 838 | Matrix dcm(3,3); |
NaotoMorita | 22:7d84b8bc20b4 | 839 | computeDcm(dcm, qhat); |
NaotoMorita | 22:7d84b8bc20b4 | 840 | Matrix vbmat = dcm*vihat; |
NaotoMorita | 22:7d84b8bc20b4 | 841 | vb.x = vbmat(1,1); |
NaotoMorita | 22:7d84b8bc20b4 | 842 | vb.y = vbmat(2,1); |
NaotoMorita | 22:7d84b8bc20b4 | 843 | vb.z = vbmat(3,1); |
NaotoMorita | 22:7d84b8bc20b4 | 844 | |
NaotoMorita | 22:7d84b8bc20b4 | 845 | } |
NaotoMorita | 22:7d84b8bc20b4 | 846 | */ |
NaotoMorita | 22:7d84b8bc20b4 | 847 | |
NaotoMorita | 22:7d84b8bc20b4 | 848 |