Eigen
Dependencies: Eigen
Dependents: optWingforHAPS_Eigen hexaTest_Eigen
ScErrStateEKF.cpp@19:3fae66745363, 2021-07-23 (annotated)
- Committer:
- NaotoMorita
- Date:
- Fri Jul 23 05:55:28 2021 +0000
- Revision:
- 19:3fae66745363
- Child:
- 20:37d3c3ee36e9
SC
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 | 19:3fae66745363 | 11 | :qhat(4,1), errState(12,1), Phat(12,12), Q(12,12), Ra(5,5), Rm(3,3), Qab(5,5),accBias(0.0f,0.0f,0.0f),gyroBias(0.0f,0.0f,0.0f) |
NaotoMorita | 19:3fae66745363 | 12 | { |
NaotoMorita | 19:3fae66745363 | 13 | qhat << 1.0f << 0.0f << 0.0f << 0.0f; |
NaotoMorita | 19:3fae66745363 | 14 | errState << 0.0f << 0.0f << 0.0f << 0.0f << 0.0f << 0.0f << 0.0f << 0.0f << 0.0f << 0.0f << 0.0f << 0.0f; |
NaotoMorita | 19:3fae66745363 | 15 | |
NaotoMorita | 19:3fae66745363 | 16 | Phat(1,1) = 0.1f; |
NaotoMorita | 19:3fae66745363 | 17 | Phat(2,2) = 0.1f; |
NaotoMorita | 19:3fae66745363 | 18 | Phat(3,3) = 0.1f; |
NaotoMorita | 19:3fae66745363 | 19 | Phat(4,4) = 0.1f; |
NaotoMorita | 19:3fae66745363 | 20 | Phat(5,5) = 0.1f; |
NaotoMorita | 19:3fae66745363 | 21 | Phat(6,6) = 0.1f; |
NaotoMorita | 19:3fae66745363 | 22 | Phat(7,7) = 0.1f; |
NaotoMorita | 19:3fae66745363 | 23 | Phat(8,8) = 0.1f; |
NaotoMorita | 19:3fae66745363 | 24 | Phat(9,9) = 0.1f; |
NaotoMorita | 19:3fae66745363 | 25 | Phat(10,10) = 0.1f; |
NaotoMorita | 19:3fae66745363 | 26 | Phat(11,11) = 0.1f; |
NaotoMorita | 19:3fae66745363 | 27 | Phat(12,12) = 0.1f; |
NaotoMorita | 19:3fae66745363 | 28 | |
NaotoMorita | 19:3fae66745363 | 29 | Q(1,1) = 0.000005f*0.25f; |
NaotoMorita | 19:3fae66745363 | 30 | Q(2,2) = 0.000005f*0.25f; |
NaotoMorita | 19:3fae66745363 | 31 | Q(3,3) = 0.000005f*0.25f; |
NaotoMorita | 19:3fae66745363 | 32 | Q(4,4) = 0.0005f; |
NaotoMorita | 19:3fae66745363 | 33 | Q(5,5) = 0.0005f; |
NaotoMorita | 19:3fae66745363 | 34 | Q(6,6) = 0.0005f; |
NaotoMorita | 19:3fae66745363 | 35 | Q(7,7) = 100.0f; |
NaotoMorita | 19:3fae66745363 | 36 | Q(8,8) = 100.0f; |
NaotoMorita | 19:3fae66745363 | 37 | Q(9,9) = 100.0f; |
NaotoMorita | 19:3fae66745363 | 38 | Q(10,10) = 100.0f; |
NaotoMorita | 19:3fae66745363 | 39 | Q(11,11) = 100.0f; |
NaotoMorita | 19:3fae66745363 | 40 | Q(12,12) = 100.0f; |
NaotoMorita | 19:3fae66745363 | 41 | |
NaotoMorita | 19:3fae66745363 | 42 | Ra(1,1) = 0.000020f; |
NaotoMorita | 19:3fae66745363 | 43 | Ra(2,2) = 0.000020f; |
NaotoMorita | 19:3fae66745363 | 44 | Ra(3,3) = 0.000020f; |
NaotoMorita | 19:3fae66745363 | 45 | Ra(4,4) = 10.0f; |
NaotoMorita | 19:3fae66745363 | 46 | Ra(5,5) = 10.0f; |
NaotoMorita | 19:3fae66745363 | 47 | |
NaotoMorita | 19:3fae66745363 | 48 | Rm(1,1) = 5.0f; |
NaotoMorita | 19:3fae66745363 | 49 | Rm(2,2) = 5.0f; |
NaotoMorita | 19:3fae66745363 | 50 | Rm(3,3) = 5.0f; |
NaotoMorita | 19:3fae66745363 | 51 | |
NaotoMorita | 19:3fae66745363 | 52 | Qab(1,1) = 100.7f; |
NaotoMorita | 19:3fae66745363 | 53 | Qab(2,2) = 100.7f; |
NaotoMorita | 19:3fae66745363 | 54 | Qab(3,3) = 100.7f; |
NaotoMorita | 19:3fae66745363 | 55 | Qab(4,4) = 0.0f; |
NaotoMorita | 19:3fae66745363 | 56 | Qab(5,5) = 0.0f; |
NaotoMorita | 19:3fae66745363 | 57 | |
NaotoMorita | 19:3fae66745363 | 58 | for(int i = 0; i<10;i++){ |
NaotoMorita | 19:3fae66745363 | 59 | histffunc[i] = 0.0f; |
NaotoMorita | 19:3fae66745363 | 60 | } |
NaotoMorita | 19:3fae66745363 | 61 | histffuncindex = 0 ; |
NaotoMorita | 19:3fae66745363 | 62 | sigma2a = 0.000020f; |
NaotoMorita | 19:3fae66745363 | 63 | |
NaotoMorita | 19:3fae66745363 | 64 | |
NaotoMorita | 19:3fae66745363 | 65 | } |
NaotoMorita | 19:3fae66745363 | 66 | |
NaotoMorita | 19:3fae66745363 | 67 | void ScErrStateEKF::updateQhat(Vector3 gyro, float att_dt) |
NaotoMorita | 19:3fae66745363 | 68 | { |
NaotoMorita | 19:3fae66745363 | 69 | gyro -= gyroBias; |
NaotoMorita | 19:3fae66745363 | 70 | Matrix A(4,4); |
NaotoMorita | 19:3fae66745363 | 71 | A << 0.0f << -0.5f*gyro.x <<-0.5f*gyro.y <<-0.5f*gyro.z |
NaotoMorita | 19:3fae66745363 | 72 | << 0.5f*gyro.x << 0.0f << 0.5f*gyro.z <<-0.5f*gyro.y |
NaotoMorita | 19:3fae66745363 | 73 | << 0.5f*gyro.y << -0.5f*gyro.z << 0.0f << 0.5f*gyro.x |
NaotoMorita | 19:3fae66745363 | 74 | << 0.5f*gyro.z << 0.5f*gyro.y <<-0.5f*gyro.x << 0.0f ; |
NaotoMorita | 19:3fae66745363 | 75 | |
NaotoMorita | 19:3fae66745363 | 76 | Matrix phi = MatrixMath::Eye(4) + A * att_dt + (0.5f*att_dt*att_dt) * A * A + (1.0f/6.0f*att_dt*att_dt*att_dt) * A * A * A; |
NaotoMorita | 19:3fae66745363 | 77 | qhat = phi * qhat; |
NaotoMorita | 19:3fae66745363 | 78 | float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qhat),qhat)); |
NaotoMorita | 19:3fae66745363 | 79 | qhat *= (1.0f/ qnorm); |
NaotoMorita | 19:3fae66745363 | 80 | } |
NaotoMorita | 19:3fae66745363 | 81 | |
NaotoMorita | 19:3fae66745363 | 82 | void ScErrStateEKF::updateErrState(Vector3 gyro, float att_dt) |
NaotoMorita | 19:3fae66745363 | 83 | { |
NaotoMorita | 19:3fae66745363 | 84 | gyro -= gyroBias; |
NaotoMorita | 19:3fae66745363 | 85 | Matrix A(12,12); |
NaotoMorita | 19:3fae66745363 | 86 | A(1,2) = gyro.z; |
NaotoMorita | 19:3fae66745363 | 87 | A(1,3) = -gyro.y; |
NaotoMorita | 19:3fae66745363 | 88 | A(2,1) = -gyro.z; |
NaotoMorita | 19:3fae66745363 | 89 | A(2,3) = gyro.x; |
NaotoMorita | 19:3fae66745363 | 90 | A(3,1) = gyro.y; |
NaotoMorita | 19:3fae66745363 | 91 | A(3,2) = -gyro.x; |
NaotoMorita | 19:3fae66745363 | 92 | A(1,4) = -0.5f; |
NaotoMorita | 19:3fae66745363 | 93 | A(2,5) = -0.5f; |
NaotoMorita | 19:3fae66745363 | 94 | A(3,6) = -0.5f; |
NaotoMorita | 19:3fae66745363 | 95 | A(10,7) = 1.0f; |
NaotoMorita | 19:3fae66745363 | 96 | A(11,8) = 1.0f; |
NaotoMorita | 19:3fae66745363 | 97 | A(12,9) = 1.0f; |
NaotoMorita | 19:3fae66745363 | 98 | |
NaotoMorita | 19:3fae66745363 | 99 | Matrix phi = MatrixMath::Eye(12) + A * att_dt + (0.5f*att_dt*att_dt) * A * A + (1.0f/6.0f*att_dt*att_dt*att_dt) * A * A * A; |
NaotoMorita | 19:3fae66745363 | 100 | Matrix Qd = Q * att_dt + 0.5f*A*Q * att_dt * att_dt+ 0.5f*Q*MatrixMath::Transpose(A) * att_dt * att_dt; |
NaotoMorita | 19:3fae66745363 | 101 | errState = phi * errState; |
NaotoMorita | 19:3fae66745363 | 102 | Phat = phi*Phat*MatrixMath::Transpose(phi)+Qd; |
NaotoMorita | 19:3fae66745363 | 103 | } |
NaotoMorita | 19:3fae66745363 | 104 | |
NaotoMorita | 19:3fae66745363 | 105 | void ScErrStateEKF::updateAccMeasures(Vector3 acc, Vector3 accref) |
NaotoMorita | 19:3fae66745363 | 106 | { |
NaotoMorita | 19:3fae66745363 | 107 | //acc -= accBias; |
NaotoMorita | 19:3fae66745363 | 108 | //acc = acc/acc.Norm(); |
NaotoMorita | 19:3fae66745363 | 109 | //accref = accref/accref.Norm(); |
NaotoMorita | 19:3fae66745363 | 110 | |
NaotoMorita | 19:3fae66745363 | 111 | Matrix dcm(3,3); |
NaotoMorita | 19:3fae66745363 | 112 | computeDcm(dcm, qhat); |
NaotoMorita | 19:3fae66745363 | 113 | Vector3 gvec(dcm(1,3)*accref.z, dcm(2,3)*accref.z, dcm(3,3)*accref.z); |
NaotoMorita | 19:3fae66745363 | 114 | Matrix H(5,12); |
NaotoMorita | 19:3fae66745363 | 115 | H(1,2) = -2.0f*gvec.z; |
NaotoMorita | 19:3fae66745363 | 116 | H(1,3) = 2.0f*gvec.y; |
NaotoMorita | 19:3fae66745363 | 117 | H(2,1) = 2.0f*gvec.z; |
NaotoMorita | 19:3fae66745363 | 118 | H(2,3) = -2.0f*gvec.x; |
NaotoMorita | 19:3fae66745363 | 119 | H(3,1) = -2.0f*gvec.y; |
NaotoMorita | 19:3fae66745363 | 120 | H(3,2) = 2.0f*gvec.x; |
NaotoMorita | 19:3fae66745363 | 121 | H(1,7) = 1.0f; |
NaotoMorita | 19:3fae66745363 | 122 | H(2,8) = 1.0f; |
NaotoMorita | 19:3fae66745363 | 123 | H(3,9) = 1.0f; |
NaotoMorita | 19:3fae66745363 | 124 | H(4,10) = 1.0f; |
NaotoMorita | 19:3fae66745363 | 125 | H(5,12) = 1.0f; |
NaotoMorita | 19:3fae66745363 | 126 | Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+Ra+Qab); |
NaotoMorita | 19:3fae66745363 | 127 | Matrix accmat(3,1); |
NaotoMorita | 19:3fae66745363 | 128 | accmat << acc.x << acc.y << acc.z; |
NaotoMorita | 19:3fae66745363 | 129 | Matrix gref(3,1); |
NaotoMorita | 19:3fae66745363 | 130 | gref << 0.0f << 0.0f << accref.z; |
NaotoMorita | 19:3fae66745363 | 131 | Matrix zacc = accmat-dcm*gref; |
NaotoMorita | 19:3fae66745363 | 132 | Matrix z(5,1); |
NaotoMorita | 19:3fae66745363 | 133 | z << zacc(1,1) << zacc(2,1) << zacc(3,1) << 0.0f << 0.0f; |
NaotoMorita | 19:3fae66745363 | 134 | Matrix corrVal = K * (z-H*errState); |
NaotoMorita | 19:3fae66745363 | 135 | errState = errState + corrVal; |
NaotoMorita | 19:3fae66745363 | 136 | Phat = (MatrixMath::Eye(12)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(12)-K*H)+K*(Ra+Qab)*MatrixMath::Transpose(K); |
NaotoMorita | 19:3fae66745363 | 137 | } |
NaotoMorita | 19:3fae66745363 | 138 | void ScErrStateEKF::updateStaticAccMeasures(Vector3 acc, Vector3 accref) |
NaotoMorita | 19:3fae66745363 | 139 | { |
NaotoMorita | 19:3fae66745363 | 140 | //acc = acc/acc.Norm(); |
NaotoMorita | 19:3fae66745363 | 141 | //accref = accref/accref.Norm(); |
NaotoMorita | 19:3fae66745363 | 142 | |
NaotoMorita | 19:3fae66745363 | 143 | Matrix dcm(3,3); |
NaotoMorita | 19:3fae66745363 | 144 | computeDcm(dcm, qhat); |
NaotoMorita | 19:3fae66745363 | 145 | Vector3 gvec(dcm(1,3)*accref.z, dcm(2,3)*accref.z, dcm(3,3)*accref.z); |
NaotoMorita | 19:3fae66745363 | 146 | Matrix H(3,9); |
NaotoMorita | 19:3fae66745363 | 147 | H(1,2) = -2.0f*gvec.z; |
NaotoMorita | 19:3fae66745363 | 148 | H(1,3) = 2.0f*gvec.y; |
NaotoMorita | 19:3fae66745363 | 149 | H(2,1) = 2.0f*gvec.z; |
NaotoMorita | 19:3fae66745363 | 150 | H(2,3) = -2.0f*gvec.x; |
NaotoMorita | 19:3fae66745363 | 151 | H(3,1) = -2.0f*gvec.y; |
NaotoMorita | 19:3fae66745363 | 152 | H(3,2) = 2.0f*gvec.x; |
NaotoMorita | 19:3fae66745363 | 153 | Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+Ra*100.0f); |
NaotoMorita | 19:3fae66745363 | 154 | Matrix accmat(3,1); |
NaotoMorita | 19:3fae66745363 | 155 | accmat << acc.x << acc.y << acc.z; |
NaotoMorita | 19:3fae66745363 | 156 | Matrix gref(3,1); |
NaotoMorita | 19:3fae66745363 | 157 | gref << 0.0f << 0.0f << accref.z; |
NaotoMorita | 19:3fae66745363 | 158 | Matrix z = accmat-dcm*gref; |
NaotoMorita | 19:3fae66745363 | 159 | errState = errState + K * (z-H*errState); |
NaotoMorita | 19:3fae66745363 | 160 | Phat = (MatrixMath::Eye(9)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(9)-K*H)+K*(Ra*100.0f)*MatrixMath::Transpose(K); |
NaotoMorita | 19:3fae66745363 | 161 | } |
NaotoMorita | 19:3fae66745363 | 162 | |
NaotoMorita | 19:3fae66745363 | 163 | |
NaotoMorita | 19:3fae66745363 | 164 | |
NaotoMorita | 19:3fae66745363 | 165 | void ScErrStateEKF::updateMagMeasures(Vector3 mag) |
NaotoMorita | 19:3fae66745363 | 166 | { |
NaotoMorita | 19:3fae66745363 | 167 | //mag = mag/mag.Norm(); |
NaotoMorita | 19:3fae66745363 | 168 | Matrix dcm(3,3); |
NaotoMorita | 19:3fae66745363 | 169 | computeDcm(dcm, qhat); |
NaotoMorita | 19:3fae66745363 | 170 | |
NaotoMorita | 19:3fae66745363 | 171 | Matrix magvec(3,1); |
NaotoMorita | 19:3fae66745363 | 172 | magvec(1,1) = mag.x; |
NaotoMorita | 19:3fae66745363 | 173 | magvec(2,1) = mag.y; |
NaotoMorita | 19:3fae66745363 | 174 | magvec(3,1) = mag.z; |
NaotoMorita | 19:3fae66745363 | 175 | |
NaotoMorita | 19:3fae66745363 | 176 | Matrix magnedvec = MatrixMath::Transpose(dcm)*magvec; |
NaotoMorita | 19:3fae66745363 | 177 | Matrix magrefmod(3,1); |
NaotoMorita | 19:3fae66745363 | 178 | magrefmod(1,1) = sqrt(magnedvec(1,1)*magnedvec(1,1)+magnedvec(2,1)*magnedvec(2,1)); |
NaotoMorita | 19:3fae66745363 | 179 | magrefmod(2,1) = 0.0f; |
NaotoMorita | 19:3fae66745363 | 180 | magrefmod(3,1) = magnedvec(3,1); |
NaotoMorita | 19:3fae66745363 | 181 | |
NaotoMorita | 19:3fae66745363 | 182 | Matrix mvec = dcm*magrefmod ; |
NaotoMorita | 19:3fae66745363 | 183 | Matrix H(3,9); |
NaotoMorita | 19:3fae66745363 | 184 | H(1,2) = -2.0f*magvec(3,1); |
NaotoMorita | 19:3fae66745363 | 185 | H(1,3) = 2.0f*magvec(2,1); |
NaotoMorita | 19:3fae66745363 | 186 | H(2,1) = 2.0f*magvec(3,1); |
NaotoMorita | 19:3fae66745363 | 187 | H(2,3) = -2.0f*magvec(1,1); |
NaotoMorita | 19:3fae66745363 | 188 | H(3,1) = -2.0f*magvec(2,1); |
NaotoMorita | 19:3fae66745363 | 189 | H(3,2) = 2.0f*magvec(1,1); |
NaotoMorita | 19:3fae66745363 | 190 | Matrix Pm(9,9); |
NaotoMorita | 19:3fae66745363 | 191 | for(int i = 1; i<4; i++){ |
NaotoMorita | 19:3fae66745363 | 192 | for(int j = 1;j<4;j++){ |
NaotoMorita | 19:3fae66745363 | 193 | Pm(i,j) = Phat(i,j); |
NaotoMorita | 19:3fae66745363 | 194 | } |
NaotoMorita | 19:3fae66745363 | 195 | } |
NaotoMorita | 19:3fae66745363 | 196 | Matrix r3(3,1); |
NaotoMorita | 19:3fae66745363 | 197 | r3 << dcm(1,3)<< dcm(2,3) << dcm(3,3); |
NaotoMorita | 19:3fae66745363 | 198 | Matrix kpart = r3*MatrixMath::Transpose(r3); |
NaotoMorita | 19:3fae66745363 | 199 | Matrix Kmod(9,9); |
NaotoMorita | 19:3fae66745363 | 200 | for(int i = 1; i<4; i++){ |
NaotoMorita | 19:3fae66745363 | 201 | for(int j = 1;j<4;j++){ |
NaotoMorita | 19:3fae66745363 | 202 | Kmod(i,j) = kpart(i,j); |
NaotoMorita | 19:3fae66745363 | 203 | } |
NaotoMorita | 19:3fae66745363 | 204 | } |
NaotoMorita | 19:3fae66745363 | 205 | |
NaotoMorita | 19:3fae66745363 | 206 | Matrix K = Kmod*(Pm*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Pm*MatrixMath::Transpose(H)+Rm); |
NaotoMorita | 19:3fae66745363 | 207 | Matrix z = magvec-mvec; |
NaotoMorita | 19:3fae66745363 | 208 | errState = errState + K * (z-H*errState); |
NaotoMorita | 19:3fae66745363 | 209 | Phat = (MatrixMath::Eye(9)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(9)-K*H)+K*(Rm)*MatrixMath::Transpose(K); |
NaotoMorita | 19:3fae66745363 | 210 | } |
NaotoMorita | 19:3fae66745363 | 211 | |
NaotoMorita | 19:3fae66745363 | 212 | void ScErrStateEKF::resetBias() |
NaotoMorita | 19:3fae66745363 | 213 | { |
NaotoMorita | 19:3fae66745363 | 214 | gyroBias.x = gyroBias.x + errState(4,1)*1.0f; |
NaotoMorita | 19:3fae66745363 | 215 | gyroBias.y = gyroBias.y + errState(5,1)*1.0f; |
NaotoMorita | 19:3fae66745363 | 216 | gyroBias.z = gyroBias.z + errState(6,1)*1.0f; |
NaotoMorita | 19:3fae66745363 | 217 | errState(4,1) = 0.0f; |
NaotoMorita | 19:3fae66745363 | 218 | errState(5,1) = 0.0f; |
NaotoMorita | 19:3fae66745363 | 219 | errState(6,1) = 0.0f; |
NaotoMorita | 19:3fae66745363 | 220 | //accBias.x = accBias.x + errState(7,1); |
NaotoMorita | 19:3fae66745363 | 221 | //accBias.y = accBias.y + errState(8,1); |
NaotoMorita | 19:3fae66745363 | 222 | //accBias.z = accBias.z + errState(9,1); |
NaotoMorita | 19:3fae66745363 | 223 | //errState(7,1) = 0.0f; |
NaotoMorita | 19:3fae66745363 | 224 | //errState(8,1) = 0.0f; |
NaotoMorita | 19:3fae66745363 | 225 | //errState(9,1) = 0.0f; |
NaotoMorita | 19:3fae66745363 | 226 | } |
NaotoMorita | 19:3fae66745363 | 227 | |
NaotoMorita | 19:3fae66745363 | 228 | void ScErrStateEKF::computeAngles(Vector3& rpy,Vector3 rpy_align) |
NaotoMorita | 19:3fae66745363 | 229 | { |
NaotoMorita | 19:3fae66745363 | 230 | Matrix qerr(4,1); |
NaotoMorita | 19:3fae66745363 | 231 | qerr << 1.0f << errState(1,1) << errState(2,1) << errState(3,1); |
NaotoMorita | 19:3fae66745363 | 232 | //float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qerr),qerr)); |
NaotoMorita | 19:3fae66745363 | 233 | //qerr *= (1.0f/ qnorm); |
NaotoMorita | 19:3fae66745363 | 234 | Matrix qest = quatmultiply(qhat, qerr); |
NaotoMorita | 19:3fae66745363 | 235 | float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qest),qest)); |
NaotoMorita | 19:3fae66745363 | 236 | qest *= (1.0f/ qnorm); |
NaotoMorita | 19:3fae66745363 | 237 | float q0 = qest( 1, 1 ); |
NaotoMorita | 19:3fae66745363 | 238 | float q1 = qest( 2, 1 ); |
NaotoMorita | 19:3fae66745363 | 239 | float q2 = qest( 3, 1 ); |
NaotoMorita | 19:3fae66745363 | 240 | float q3 = qest( 4, 1 ); |
NaotoMorita | 19:3fae66745363 | 241 | rpy.x = atan2f(q0*q1 + q2*q3, 0.5f - q1*q1 - q2*q2)-rpy_align.x; |
NaotoMorita | 19:3fae66745363 | 242 | rpy.y = asinf(-2.0f * (q1*q3 - q0*q2))-rpy_align.y; |
NaotoMorita | 19:3fae66745363 | 243 | rpy.z = atan2f(q1*q2 + q0*q3, 0.5f - q2*q2 - q3*q3); |
NaotoMorita | 19:3fae66745363 | 244 | |
NaotoMorita | 19:3fae66745363 | 245 | } |
NaotoMorita | 19:3fae66745363 | 246 | |
NaotoMorita | 19:3fae66745363 | 247 | void ScErrStateEKF::fuseErr2Qhat() |
NaotoMorita | 19:3fae66745363 | 248 | { |
NaotoMorita | 19:3fae66745363 | 249 | Matrix qerr(4,1); |
NaotoMorita | 19:3fae66745363 | 250 | qerr << 1.0f << errState(1,1) << errState(2,1) << errState(3,1); |
NaotoMorita | 19:3fae66745363 | 251 | //float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qerr),qerr)); |
NaotoMorita | 19:3fae66745363 | 252 | //qerr *= (1.0f/ qnorm); |
NaotoMorita | 19:3fae66745363 | 253 | qhat = quatmultiply(qhat, qerr); |
NaotoMorita | 19:3fae66745363 | 254 | errState(1,1) = 0.0f; |
NaotoMorita | 19:3fae66745363 | 255 | errState(2,1) = 0.0f; |
NaotoMorita | 19:3fae66745363 | 256 | errState(3,1) = 0.0f; |
NaotoMorita | 19:3fae66745363 | 257 | float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qhat),qhat)); |
NaotoMorita | 19:3fae66745363 | 258 | qhat *= (1.0f/ qnorm); |
NaotoMorita | 19:3fae66745363 | 259 | } |
NaotoMorita | 19:3fae66745363 | 260 | |
NaotoMorita | 19:3fae66745363 | 261 | Matrix ScErrStateEKF::quatmultiply(Matrix q, Matrix r) |
NaotoMorita | 19:3fae66745363 | 262 | { |
NaotoMorita | 19:3fae66745363 | 263 | Matrix qout(4,1); |
NaotoMorita | 19:3fae66745363 | 264 | 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 | 265 | 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 | 266 | 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 | 267 | 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 | 268 | float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qout),qout)); |
NaotoMorita | 19:3fae66745363 | 269 | qout *= (1.0f/ qnorm); |
NaotoMorita | 19:3fae66745363 | 270 | return qout; |
NaotoMorita | 19:3fae66745363 | 271 | } |
NaotoMorita | 19:3fae66745363 | 272 | |
NaotoMorita | 19:3fae66745363 | 273 | void ScErrStateEKF::computeDcm(Matrix& dcm, Matrix quat) |
NaotoMorita | 19:3fae66745363 | 274 | { |
NaotoMorita | 19:3fae66745363 | 275 | |
NaotoMorita | 19:3fae66745363 | 276 | float q0 = quat( 1, 1 ); |
NaotoMorita | 19:3fae66745363 | 277 | float q1 = quat( 2, 1 ); |
NaotoMorita | 19:3fae66745363 | 278 | float q2 = quat( 3, 1 ); |
NaotoMorita | 19:3fae66745363 | 279 | float q3 = quat( 4, 1 ); |
NaotoMorita | 19:3fae66745363 | 280 | |
NaotoMorita | 19:3fae66745363 | 281 | dcm(1,1) = q0*q0 + q1*q1 - q2*q2 - q3*q3; |
NaotoMorita | 19:3fae66745363 | 282 | dcm(1,2) = 2*(q1*q2 + q0*q3); |
NaotoMorita | 19:3fae66745363 | 283 | dcm(1,3) = 2*(q1*q3 - q0*q2); |
NaotoMorita | 19:3fae66745363 | 284 | dcm(2,1) = 2*(q1*q2 - q0*q3); |
NaotoMorita | 19:3fae66745363 | 285 | dcm(2,2) = q0*q0 - q1*q1 + q2*q2 - q3*q3; |
NaotoMorita | 19:3fae66745363 | 286 | dcm(2,3) = 2*(q2*q3 + q0*q1); |
NaotoMorita | 19:3fae66745363 | 287 | dcm(3,1) = 2*(q1*q3 + q0*q2); |
NaotoMorita | 19:3fae66745363 | 288 | dcm(3,2) = 2*(q2*q3 - q0*q1); |
NaotoMorita | 19:3fae66745363 | 289 | dcm(3,3) = q0*q0 - q1*q1 - q2*q2 + q3*q3; |
NaotoMorita | 19:3fae66745363 | 290 | |
NaotoMorita | 19:3fae66745363 | 291 | } |
NaotoMorita | 19:3fae66745363 | 292 | |
NaotoMorita | 19:3fae66745363 | 293 | void ScErrStateEKF::defineQhat(Vector3 align){ |
NaotoMorita | 19:3fae66745363 | 294 | float cos_z_2 = cosf(0.5f*align.z); |
NaotoMorita | 19:3fae66745363 | 295 | float cos_y_2 = cosf(0.5f*align.y); |
NaotoMorita | 19:3fae66745363 | 296 | float cos_x_2 = cosf(0.5f*align.x); |
NaotoMorita | 19:3fae66745363 | 297 | |
NaotoMorita | 19:3fae66745363 | 298 | float sin_z_2 = sinf(0.5f*align.z); |
NaotoMorita | 19:3fae66745363 | 299 | float sin_y_2 = sinf(0.5f*align.y); |
NaotoMorita | 19:3fae66745363 | 300 | float sin_x_2 = sinf(0.5f*align.x); |
NaotoMorita | 19:3fae66745363 | 301 | |
NaotoMorita | 19:3fae66745363 | 302 | // and now compute quaternion |
NaotoMorita | 19:3fae66745363 | 303 | qhat(1,1) = cos_z_2*cos_y_2*cos_x_2 + sin_z_2*sin_y_2*sin_x_2; |
NaotoMorita | 19:3fae66745363 | 304 | qhat(2,1) = cos_z_2*cos_y_2*sin_x_2 - sin_z_2*sin_y_2*cos_x_2; |
NaotoMorita | 19:3fae66745363 | 305 | qhat(3,1) = cos_z_2*sin_y_2*cos_x_2 + sin_z_2*cos_y_2*sin_x_2; |
NaotoMorita | 19:3fae66745363 | 306 | qhat(4,1) = sin_z_2*cos_y_2*cos_x_2 - cos_z_2*sin_y_2*sin_x_2; |
NaotoMorita | 19:3fae66745363 | 307 | } |
NaotoMorita | 19:3fae66745363 | 308 | |
NaotoMorita | 19:3fae66745363 | 309 | void ScErrStateEKF::triad(Vector3 fb, Vector3 fn, Vector3 mb, Vector3 mn){ |
NaotoMorita | 19:3fae66745363 | 310 | Matrix W1(3,1); |
NaotoMorita | 19:3fae66745363 | 311 | W1 << fb.x << fb.y << fb.z; |
NaotoMorita | 19:3fae66745363 | 312 | Matrix W2(3,1); |
NaotoMorita | 19:3fae66745363 | 313 | W2 << mb.x << mb.y << mb.z; |
NaotoMorita | 19:3fae66745363 | 314 | |
NaotoMorita | 19:3fae66745363 | 315 | Matrix V1(3,1); |
NaotoMorita | 19:3fae66745363 | 316 | V1 << fn.x << fn.y << fn.z; |
NaotoMorita | 19:3fae66745363 | 317 | Matrix V2(3,1); |
NaotoMorita | 19:3fae66745363 | 318 | V2 << mn.x << mn.y << mn.z; |
NaotoMorita | 19:3fae66745363 | 319 | |
NaotoMorita | 19:3fae66745363 | 320 | |
NaotoMorita | 19:3fae66745363 | 321 | Matrix Ou2(3,1); |
NaotoMorita | 19:3fae66745363 | 322 | Ou2 << W1( 2, 1 )*W2( 3, 1 )-W1( 3, 1 )*W2( 2, 1 ) << W1( 3, 1 )*W2( 1, 1 )-W1( 1, 1 )*W2( 3, 1 ) << W1( 1, 1 )*W2( 2, 1 )-W1( 2, 1 )*W2( 1, 1 ); |
NaotoMorita | 19:3fae66745363 | 323 | Ou2 *= 1.0f/sqrt(MatrixMath::dot(MatrixMath::Transpose(Ou2),Ou2)); |
NaotoMorita | 19:3fae66745363 | 324 | Matrix Ou3(3,1); |
NaotoMorita | 19:3fae66745363 | 325 | Ou3 << W1( 2, 1 )*Ou2( 3, 1 )-W1( 3, 1 )*Ou2( 2, 1 ) << W1( 3, 1 )*Ou2( 1, 1 )-W1( 1, 1 )*Ou2( 3, 1 ) << W1( 1, 1 )*Ou2( 2, 1 )-W1( 2, 1 )*Ou2( 1, 1 ); |
NaotoMorita | 19:3fae66745363 | 326 | Ou3 *= 1.0f/sqrt(MatrixMath::dot(MatrixMath::Transpose(Ou3),Ou3)); |
NaotoMorita | 19:3fae66745363 | 327 | Matrix R2(3,1); |
NaotoMorita | 19:3fae66745363 | 328 | R2 << V1( 2, 1 )*V2( 3, 1 )-V1( 3, 1 )*V2( 2, 1 ) << V1( 3, 1 )*V2( 1, 1 )-V1( 1, 1 )*V2( 3, 1 ) << V1( 1, 1 )*V2( 2, 1 )-V1( 2, 1 )*V2( 1, 1 ); |
NaotoMorita | 19:3fae66745363 | 329 | R2 *= 1.0f/sqrt(MatrixMath::dot(MatrixMath::Transpose(R2),R2)); |
NaotoMorita | 19:3fae66745363 | 330 | Matrix R3(3,1); |
NaotoMorita | 19:3fae66745363 | 331 | R3 << V1( 2, 1 )*R2( 3, 1 )-V1( 3, 1 )*R2( 2, 1 ) << V1( 3, 1 )*R2( 1, 1 )-V1( 1, 1 )*R2( 3, 1 ) << V1( 1, 1 )*R2( 2, 1 )-V1( 2, 1 )*R2( 1, 1 ); |
NaotoMorita | 19:3fae66745363 | 332 | R3 *= 1.0f/sqrt(MatrixMath::dot(MatrixMath::Transpose(R3),R3)); |
NaotoMorita | 19:3fae66745363 | 333 | |
NaotoMorita | 19:3fae66745363 | 334 | Matrix Mou(3,3); |
NaotoMorita | 19:3fae66745363 | 335 | Mou << W1( 1, 1 ) << Ou2( 1, 1 ) << Ou3( 1, 1 ) |
NaotoMorita | 19:3fae66745363 | 336 | << W1( 2, 1 ) << Ou2( 2, 1 ) << Ou3( 2, 1 ) |
NaotoMorita | 19:3fae66745363 | 337 | << W1( 3, 1 ) << Ou2( 3, 1 ) << Ou3( 3, 1 ); |
NaotoMorita | 19:3fae66745363 | 338 | Matrix Mr(3,3); |
NaotoMorita | 19:3fae66745363 | 339 | Mr << V1( 1, 1 ) << R2( 1, 1 ) << R3( 1, 1 ) |
NaotoMorita | 19:3fae66745363 | 340 | << V1( 2, 1 ) << R2( 2, 1 ) << R3( 2, 1 ) |
NaotoMorita | 19:3fae66745363 | 341 | << V1( 3, 1 ) << R2( 3, 1 ) << R3( 3, 1 ); |
NaotoMorita | 19:3fae66745363 | 342 | |
NaotoMorita | 19:3fae66745363 | 343 | Matrix Cbn = Mr*MatrixMath::Transpose(Mou); |
NaotoMorita | 19:3fae66745363 | 344 | |
NaotoMorita | 19:3fae66745363 | 345 | float sqtrp1 = sqrt(1.0f+Cbn( 1, 1 )+Cbn( 2, 2 )+Cbn( 3, 3 )); |
NaotoMorita | 19:3fae66745363 | 346 | |
NaotoMorita | 19:3fae66745363 | 347 | qhat(1,1) = 0.5f*sqtrp1; |
NaotoMorita | 19:3fae66745363 | 348 | qhat(2,1) = -(Cbn( 2, 3 )-Cbn( 3, 2 ))/2.0f/sqtrp1; |
NaotoMorita | 19:3fae66745363 | 349 | qhat(3,1) = -(Cbn( 3, 1 )-Cbn( 1, 3 ))/2.0f/sqtrp1; |
NaotoMorita | 19:3fae66745363 | 350 | qhat(4,1) = -(Cbn( 1, 2 )-Cbn( 2, 1 ))/2.0f/sqtrp1; |
NaotoMorita | 19:3fae66745363 | 351 | |
NaotoMorita | 19:3fae66745363 | 352 | float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qhat),qhat)); |
NaotoMorita | 19:3fae66745363 | 353 | qhat *= (1.0f/ qnorm); |
NaotoMorita | 19:3fae66745363 | 354 | } |
NaotoMorita | 19:3fae66745363 | 355 | |
NaotoMorita | 19:3fae66745363 | 356 | |
NaotoMorita | 19:3fae66745363 | 357 | Vector3 ScErrStateEKF::calcDynAcc(Vector3 acc, Vector3 accref) |
NaotoMorita | 19:3fae66745363 | 358 | { |
NaotoMorita | 19:3fae66745363 | 359 | Matrix dcm(3,3); |
NaotoMorita | 19:3fae66745363 | 360 | computeDcm(dcm, qhat); |
NaotoMorita | 19:3fae66745363 | 361 | float _x, _y, _z; |
NaotoMorita | 19:3fae66745363 | 362 | _x = acc.x-(dcm( 1, 1 )*accref.x+dcm( 1, 2 )*accref.y+dcm( 1, 3 )*accref.z); |
NaotoMorita | 19:3fae66745363 | 363 | _y = acc.y-(dcm( 2, 1 )*accref.x+dcm( 2, 2 )*accref.y+dcm( 2, 3 )*accref.z); |
NaotoMorita | 19:3fae66745363 | 364 | _z = acc.z-(dcm( 3, 1 )*accref.x+dcm( 3, 2 )*accref.y+dcm( 3, 3 )*accref.z); |
NaotoMorita | 19:3fae66745363 | 365 | return Vector3(_x, _y, _z); |
NaotoMorita | 19:3fae66745363 | 366 | } |
NaotoMorita | 19:3fae66745363 | 367 | |
NaotoMorita | 19:3fae66745363 | 368 | |
NaotoMorita | 19:3fae66745363 | 369 | bool ScErrStateEKF::determinDynStatus(Vector3 acc, Vector3 accref) |
NaotoMorita | 19:3fae66745363 | 370 | { |
NaotoMorita | 19:3fae66745363 | 371 | histffunc[histffuncindex] = acc.Norm()*acc.Norm()-accref.Norm()*accref.Norm()-3.0f*sigma2a; |
NaotoMorita | 19:3fae66745363 | 372 | if(histffuncindex<9){ |
NaotoMorita | 19:3fae66745363 | 373 | histffuncindex += 1; |
NaotoMorita | 19:3fae66745363 | 374 | }else{ |
NaotoMorita | 19:3fae66745363 | 375 | histffuncindex =0; |
NaotoMorita | 19:3fae66745363 | 376 | } |
NaotoMorita | 19:3fae66745363 | 377 | aveffunc = 0; |
NaotoMorita | 19:3fae66745363 | 378 | for(int i = 1;i<10;i++){ |
NaotoMorita | 19:3fae66745363 | 379 | aveffunc += 1.0f/10.0f*histffunc[i]; |
NaotoMorita | 19:3fae66745363 | 380 | } |
NaotoMorita | 19:3fae66745363 | 381 | sigma2f = 1.0f/10.0f*(6.0f*sigma2a*sigma2a+4.0f*accref.Norm()*accref.Norm()*sigma2a); |
NaotoMorita | 19:3fae66745363 | 382 | |
NaotoMorita | 19:3fae66745363 | 383 | |
NaotoMorita | 19:3fae66745363 | 384 | dynacc = calcDynAcc(acc,accref); |
NaotoMorita | 19:3fae66745363 | 385 | bool dynCase = true; |
NaotoMorita | 19:3fae66745363 | 386 | if((dynacc.Norm()<0.005f) && (abs(acc.Norm()-accref.Norm())<0.005f)){dynCase = false;} |
NaotoMorita | 19:3fae66745363 | 387 | if(aveffunc<sqrt(sigma2f/0.99f) && abs(acc.Norm()-accref.Norm())<0.001f){dynCase = false;} |
NaotoMorita | 19:3fae66745363 | 388 | return dynCase; |
NaotoMorita | 19:3fae66745363 | 389 | |
NaotoMorita | 19:3fae66745363 | 390 | } |
NaotoMorita | 19:3fae66745363 | 391 | |
NaotoMorita | 19:3fae66745363 | 392 |