EKF class
Dependencies: MatrixMath Vector3
HAPS_EKF.cpp@2:771eed5f655a, 2021-05-31 (annotated)
- Committer:
- cocorlow
- Date:
- Mon May 31 07:27:20 2021 +0000
- Revision:
- 2:771eed5f655a
- Parent:
- 0:de56252b419e
rpy
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
cocorlow | 0:de56252b419e | 1 | #include "HAPS_EKF.hpp" |
cocorlow | 0:de56252b419e | 2 | #include "Matrix.h" |
cocorlow | 0:de56252b419e | 3 | #include "MatrixMath.h" |
cocorlow | 0:de56252b419e | 4 | #include <cmath> |
cocorlow | 0:de56252b419e | 5 | #include "Vector3.hpp" |
cocorlow | 0:de56252b419e | 6 | |
cocorlow | 0:de56252b419e | 7 | |
cocorlow | 0:de56252b419e | 8 | using namespace std; |
cocorlow | 0:de56252b419e | 9 | |
cocorlow | 0:de56252b419e | 10 | HAPS_EKF::HAPS_EKF() |
cocorlow | 0:de56252b419e | 11 | :qhat(4,1), qhat_gyro(4,1), Phat(4,4), Qgyro(3,3), Racc(3,3), Rmag(3,3), D(3,3) |
cocorlow | 0:de56252b419e | 12 | { |
cocorlow | 0:de56252b419e | 13 | qhat << 1 << 0 << 0 << 0; |
cocorlow | 0:de56252b419e | 14 | |
cocorlow | 0:de56252b419e | 15 | Phat.add(1,1,0.05); |
cocorlow | 0:de56252b419e | 16 | Phat.add(2,2,0.05); |
cocorlow | 0:de56252b419e | 17 | Phat.add(3,3,0.05); |
cocorlow | 0:de56252b419e | 18 | Phat.add(4,4,0.05); |
cocorlow | 0:de56252b419e | 19 | |
cocorlow | 0:de56252b419e | 20 | D.add(1,1,1.0); |
cocorlow | 0:de56252b419e | 21 | D.add(2,2,1.0); |
cocorlow | 0:de56252b419e | 22 | D.add(3,3,1.0); |
cocorlow | 0:de56252b419e | 23 | |
cocorlow | 0:de56252b419e | 24 | Qgyro.add(1,1,0.0224); |
cocorlow | 0:de56252b419e | 25 | Qgyro.add(2,2,0.0224); |
cocorlow | 0:de56252b419e | 26 | Qgyro.add(3,3,0.0224); |
cocorlow | 0:de56252b419e | 27 | |
cocorlow | 0:de56252b419e | 28 | Racc.add(1,1,0.0330*200); |
cocorlow | 0:de56252b419e | 29 | Racc.add(2,2,0.0330*200); |
cocorlow | 0:de56252b419e | 30 | Racc.add(3,3,0.0330*200); |
cocorlow | 0:de56252b419e | 31 | |
cocorlow | 0:de56252b419e | 32 | Rmag.add(1,1,1.0); |
cocorlow | 0:de56252b419e | 33 | Rmag.add(2,2,1.0); |
cocorlow | 0:de56252b419e | 34 | Rmag.add(3,3,1.0); |
cocorlow | 0:de56252b419e | 35 | } |
cocorlow | 0:de56252b419e | 36 | |
cocorlow | 0:de56252b419e | 37 | void HAPS_EKF::updateBetweenMeasures(Vector3 gyro, float att_dt) |
cocorlow | 0:de56252b419e | 38 | { |
cocorlow | 0:de56252b419e | 39 | float q0 = qhat.getNumber( 1, 1 ); |
cocorlow | 0:de56252b419e | 40 | float q1 = qhat.getNumber( 2, 1 ); |
cocorlow | 0:de56252b419e | 41 | float q2 = qhat.getNumber( 3, 1 ); |
cocorlow | 0:de56252b419e | 42 | float q3 = qhat.getNumber( 4, 1 ); |
cocorlow | 0:de56252b419e | 43 | |
cocorlow | 0:de56252b419e | 44 | Matrix B(4,3); |
cocorlow | 0:de56252b419e | 45 | B << q1 << q2 << q3 |
cocorlow | 0:de56252b419e | 46 | <<-q0 << q3 <<-q2 |
cocorlow | 0:de56252b419e | 47 | <<-q3 <<-q0 << q1 |
cocorlow | 0:de56252b419e | 48 | << q2 <<-q1 <<-q0; |
cocorlow | 0:de56252b419e | 49 | B *= 0.5f; |
cocorlow | 0:de56252b419e | 50 | |
cocorlow | 0:de56252b419e | 51 | Matrix phi(4,4); |
cocorlow | 0:de56252b419e | 52 | phi << 1.0 << -gyro.x*0.5*att_dt <<-gyro.y*0.5*att_dt <<-gyro.z*0.5*att_dt |
cocorlow | 0:de56252b419e | 53 | << gyro.x*0.5*att_dt << 1.0 << gyro.z*0.5*att_dt <<-gyro.y*0.5*att_dt |
cocorlow | 0:de56252b419e | 54 | << gyro.y*0.5*att_dt << -gyro.z*0.5*att_dt << 1.0 << gyro.x*0.5*att_dt |
cocorlow | 0:de56252b419e | 55 | << gyro.z*0.5*att_dt << gyro.y*0.5*att_dt <<-gyro.x*0.5*att_dt << 1.0; |
cocorlow | 0:de56252b419e | 56 | |
cocorlow | 0:de56252b419e | 57 | qhat = phi*qhat; |
cocorlow | 0:de56252b419e | 58 | float qnorm; |
cocorlow | 0:de56252b419e | 59 | qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qhat),qhat)); |
cocorlow | 0:de56252b419e | 60 | qhat *= (1.0f/ qnorm); |
cocorlow | 0:de56252b419e | 61 | |
cocorlow | 0:de56252b419e | 62 | qhat_gyro = phi*qhat_gyro; |
cocorlow | 0:de56252b419e | 63 | qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qhat_gyro),qhat_gyro)); |
cocorlow | 0:de56252b419e | 64 | qhat_gyro *= (1.0f/ qnorm); |
cocorlow | 0:de56252b419e | 65 | |
cocorlow | 0:de56252b419e | 66 | Phat = phi*Phat*MatrixMath::Transpose(phi)+B*Qgyro*MatrixMath::Transpose(B); |
cocorlow | 0:de56252b419e | 67 | |
cocorlow | 0:de56252b419e | 68 | q0 = qhat.getNumber( 1, 1 ); |
cocorlow | 0:de56252b419e | 69 | q1 = qhat.getNumber( 2, 1 ); |
cocorlow | 0:de56252b419e | 70 | q2 = qhat.getNumber( 3, 1 ); |
cocorlow | 0:de56252b419e | 71 | q3 = qhat.getNumber( 4, 1 ); |
cocorlow | 0:de56252b419e | 72 | |
cocorlow | 0:de56252b419e | 73 | D.add(1,1,q0*q0 + q1*q1 - q2*q2 - q3*q3); |
cocorlow | 0:de56252b419e | 74 | D.add(1,2,2*(q1*q2 + q0*q3)); |
cocorlow | 0:de56252b419e | 75 | D.add(1,3,2*(q1*q3 - q0*q2)); |
cocorlow | 0:de56252b419e | 76 | D.add(2,1,2*(q1*q2 - q0*q3)); |
cocorlow | 0:de56252b419e | 77 | D.add(2,2,q0*q0 - q1*q1 + q2*q2 - q3*q3); |
cocorlow | 0:de56252b419e | 78 | D.add(2,3,2*(q2*q3 + q0*q1)); |
cocorlow | 0:de56252b419e | 79 | D.add(3,1,2*(q1*q3 + q0*q2)); |
cocorlow | 0:de56252b419e | 80 | D.add(3,2,2*(q2*q3 - q0*q1)); |
cocorlow | 0:de56252b419e | 81 | D.add(3,3,q0*q0 - q1*q1 - q2*q2 + q3*q3); |
cocorlow | 0:de56252b419e | 82 | } |
cocorlow | 0:de56252b419e | 83 | |
cocorlow | 0:de56252b419e | 84 | void HAPS_EKF::updateAcrossMeasures(Vector3 _v, Vector3 _u, Matrix& R) |
cocorlow | 0:de56252b419e | 85 | { |
cocorlow | 0:de56252b419e | 86 | Matrix u(3,1); |
cocorlow | 0:de56252b419e | 87 | Matrix v(3,1); |
cocorlow | 0:de56252b419e | 88 | |
cocorlow | 0:de56252b419e | 89 | u << _u.x << _u.y << _u.z; |
cocorlow | 0:de56252b419e | 90 | v << _v.x << _v.y << _v.z; |
cocorlow | 0:de56252b419e | 91 | |
cocorlow | 0:de56252b419e | 92 | float q0 = qhat.getNumber( 1, 1 ); |
cocorlow | 0:de56252b419e | 93 | float q1 = qhat.getNumber( 2, 1 ); |
cocorlow | 0:de56252b419e | 94 | float q2 = qhat.getNumber( 3, 1 ); |
cocorlow | 0:de56252b419e | 95 | float q3 = qhat.getNumber( 4, 1 ); |
cocorlow | 0:de56252b419e | 96 | |
cocorlow | 0:de56252b419e | 97 | Matrix A1(3,3); |
cocorlow | 0:de56252b419e | 98 | A1 << q0 << q3 << -q2 |
cocorlow | 0:de56252b419e | 99 | <<-q3 << q0 << q1 |
cocorlow | 0:de56252b419e | 100 | <<q2 <<-q1 <<q0; |
cocorlow | 0:de56252b419e | 101 | A1 *= 2.0f; |
cocorlow | 0:de56252b419e | 102 | |
cocorlow | 0:de56252b419e | 103 | Matrix A2(3,3); |
cocorlow | 0:de56252b419e | 104 | A2 << q1 << q2 << q3 |
cocorlow | 0:de56252b419e | 105 | << q2 <<-q1 << q0 |
cocorlow | 0:de56252b419e | 106 | << q3 <<-q0 <<-q1; |
cocorlow | 0:de56252b419e | 107 | A2 *= 2.0f; |
cocorlow | 0:de56252b419e | 108 | |
cocorlow | 0:de56252b419e | 109 | Matrix A3(3,3); |
cocorlow | 0:de56252b419e | 110 | A3 <<-q2 << q1 <<-q0 |
cocorlow | 0:de56252b419e | 111 | << q1 << q2 << q3 |
cocorlow | 0:de56252b419e | 112 | << q0 << q3 <<-q2; |
cocorlow | 0:de56252b419e | 113 | A3 *= 2.0f; |
cocorlow | 0:de56252b419e | 114 | |
cocorlow | 0:de56252b419e | 115 | Matrix A4(3,3); |
cocorlow | 0:de56252b419e | 116 | A4 <<-q3 << q0 << q1 |
cocorlow | 0:de56252b419e | 117 | <<-q0 <<-q3 << q2 |
cocorlow | 0:de56252b419e | 118 | << q1 << q2 << q3; |
cocorlow | 0:de56252b419e | 119 | A4 *= 2.0f; |
cocorlow | 0:de56252b419e | 120 | |
cocorlow | 0:de56252b419e | 121 | Matrix H(3,4); |
cocorlow | 0:de56252b419e | 122 | |
cocorlow | 0:de56252b419e | 123 | Matrix ab1(A1*u); |
cocorlow | 0:de56252b419e | 124 | Matrix ab2(A2*u); |
cocorlow | 0:de56252b419e | 125 | Matrix ab3(A3*u); |
cocorlow | 0:de56252b419e | 126 | Matrix ab4(A4*u); |
cocorlow | 0:de56252b419e | 127 | |
cocorlow | 0:de56252b419e | 128 | H << ab1.getNumber( 1, 1 ) << ab2.getNumber( 1, 1 ) << ab3.getNumber( 1, 1 ) << ab4.getNumber( 1, 1 ) |
cocorlow | 0:de56252b419e | 129 | << ab1.getNumber( 2, 1 ) << ab2.getNumber( 2, 1 ) << ab3.getNumber( 2, 1 ) << ab4.getNumber( 2, 1 ) |
cocorlow | 0:de56252b419e | 130 | << ab1.getNumber( 3, 1 ) << ab2.getNumber( 3, 1 ) << ab3.getNumber( 3, 1 ) << ab4.getNumber( 3, 1 ); |
cocorlow | 0:de56252b419e | 131 | |
cocorlow | 0:de56252b419e | 132 | |
cocorlow | 0:de56252b419e | 133 | Matrix K(4,3); |
cocorlow | 0:de56252b419e | 134 | K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R); |
cocorlow | 0:de56252b419e | 135 | |
cocorlow | 0:de56252b419e | 136 | Matrix dq(4,1); |
cocorlow | 0:de56252b419e | 137 | dq = K*(v-D*u); |
cocorlow | 0:de56252b419e | 138 | qhat = qhat+dq; |
cocorlow | 0:de56252b419e | 139 | |
cocorlow | 0:de56252b419e | 140 | float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qhat),qhat)); |
cocorlow | 0:de56252b419e | 141 | qhat *= (1.0f/ qnorm); |
cocorlow | 0:de56252b419e | 142 | |
cocorlow | 0:de56252b419e | 143 | Matrix eye4(4,4); |
cocorlow | 0:de56252b419e | 144 | eye4 << 1 << 0 << 0 << 0 |
cocorlow | 0:de56252b419e | 145 | << 0 << 1 << 0 << 0 |
cocorlow | 0:de56252b419e | 146 | << 0 << 0 << 1 << 0 |
cocorlow | 0:de56252b419e | 147 | << 0 << 0 << 0 << 1; |
cocorlow | 0:de56252b419e | 148 | Phat = (eye4-K*H)*Phat*MatrixMath::Transpose(eye4-K*H)+K*R*MatrixMath::Transpose(K); |
cocorlow | 0:de56252b419e | 149 | } |
cocorlow | 0:de56252b419e | 150 | |
cocorlow | 2:771eed5f655a | 151 | void HAPS_EKF::computeAngles(Vector3& rpy, Vector3& rpy_g, Vector3 rpy_align) |
cocorlow | 0:de56252b419e | 152 | { |
cocorlow | 0:de56252b419e | 153 | float q0 = qhat.getNumber( 1, 1 ); |
cocorlow | 0:de56252b419e | 154 | float q1 = qhat.getNumber( 2, 1 ); |
cocorlow | 0:de56252b419e | 155 | float q2 = qhat.getNumber( 3, 1 ); |
cocorlow | 0:de56252b419e | 156 | float q3 = qhat.getNumber( 4, 1 ); |
cocorlow | 2:771eed5f655a | 157 | rpy.x = atan2f(q0*q1 + q2*q3, 0.5f - q1*q1 - q2*q2)-rpy_align.x; |
cocorlow | 2:771eed5f655a | 158 | rpy.y = asinf(-2.0f * (q1*q3 - q0*q2))-rpy_align.y; |
cocorlow | 2:771eed5f655a | 159 | rpy.z = atan2f(q1*q2 + q0*q3, 0.5f - q2*q2 - q3*q3); |
cocorlow | 0:de56252b419e | 160 | |
cocorlow | 0:de56252b419e | 161 | q0 = qhat_gyro.getNumber( 1, 1 ); |
cocorlow | 0:de56252b419e | 162 | q1 = qhat_gyro.getNumber( 2, 1 ); |
cocorlow | 0:de56252b419e | 163 | q2 = qhat_gyro.getNumber( 3, 1 ); |
cocorlow | 0:de56252b419e | 164 | q3 = qhat_gyro.getNumber( 4, 1 ); |
cocorlow | 2:771eed5f655a | 165 | rpy_g.x = atan2f(q0*q1 + q2*q3, 0.5f - q1*q1 - q2*q2)-rpy_align.x; |
cocorlow | 2:771eed5f655a | 166 | rpy_g.y = asinf(-2.0f * (q1*q3 - q0*q2))-rpy_align.y; |
cocorlow | 2:771eed5f655a | 167 | rpy_g.z = atan2f(q1*q2 + q0*q3, 0.5f - q2*q2 - q3*q3); |
cocorlow | 0:de56252b419e | 168 | |
cocorlow | 0:de56252b419e | 169 | } |
cocorlow | 0:de56252b419e | 170 | |
cocorlow | 0:de56252b419e | 171 | void HAPS_EKF::triad(Vector3 fb, Vector3 fn, Vector3 mb, Vector3 mn){ |
cocorlow | 0:de56252b419e | 172 | Matrix W1(3,1); |
cocorlow | 0:de56252b419e | 173 | W1 << fb.x << fb.y << fb.z; |
cocorlow | 0:de56252b419e | 174 | Matrix W2(3,1); |
cocorlow | 0:de56252b419e | 175 | W2 << mb.x << mb.y << mb.z; |
cocorlow | 0:de56252b419e | 176 | |
cocorlow | 0:de56252b419e | 177 | Matrix V1(3,1); |
cocorlow | 0:de56252b419e | 178 | V1 << fn.x << fn.y << fn.z; |
cocorlow | 0:de56252b419e | 179 | Matrix V2(3,1); |
cocorlow | 0:de56252b419e | 180 | V2 << mn.x << mn.y << mn.z; |
cocorlow | 0:de56252b419e | 181 | |
cocorlow | 0:de56252b419e | 182 | |
cocorlow | 0:de56252b419e | 183 | Matrix Ou2(3,1); |
cocorlow | 0:de56252b419e | 184 | Ou2 << W1.getNumber( 2, 1 )*W2.getNumber( 3, 1 )-W1.getNumber( 3, 1 )*W2.getNumber( 2, 1 ) << W1.getNumber( 3, 1 )*W2.getNumber( 1, 1 )-W1.getNumber( 1, 1 )*W2.getNumber( 3, 1 ) << W1.getNumber( 1, 1 )*W2.getNumber( 2, 1 )-W1.getNumber( 2, 1 )*W2.getNumber( 1, 1 ); |
cocorlow | 0:de56252b419e | 185 | Ou2 *= 1.0/sqrt(MatrixMath::dot(MatrixMath::Transpose(Ou2),Ou2)); |
cocorlow | 0:de56252b419e | 186 | Matrix Ou3(3,1); |
cocorlow | 0:de56252b419e | 187 | Ou3 << W1.getNumber( 2, 1 )*Ou2.getNumber( 3, 1 )-W1.getNumber( 3, 1 )*Ou2.getNumber( 2, 1 ) << W1.getNumber( 3, 1 )*Ou2.getNumber( 1, 1 )-W1.getNumber( 1, 1 )*Ou2.getNumber( 3, 1 ) << W1.getNumber( 1, 1 )*Ou2.getNumber( 2, 1 )-W1.getNumber( 2, 1 )*Ou2.getNumber( 1, 1 ); |
cocorlow | 0:de56252b419e | 188 | Ou3 *= 1.0/sqrt(MatrixMath::dot(MatrixMath::Transpose(Ou3),Ou3)); |
cocorlow | 0:de56252b419e | 189 | Matrix R2(3,1); |
cocorlow | 0:de56252b419e | 190 | R2 << V1.getNumber( 2, 1 )*V2.getNumber( 3, 1 )-V1.getNumber( 3, 1 )*V2.getNumber( 2, 1 ) << V1.getNumber( 3, 1 )*V2.getNumber( 1, 1 )-V1.getNumber( 1, 1 )*V2.getNumber( 3, 1 ) << V1.getNumber( 1, 1 )*V2.getNumber( 2, 1 )-V1.getNumber( 2, 1 )*V2.getNumber( 1, 1 ); |
cocorlow | 0:de56252b419e | 191 | R2 *= 1.0/sqrt(MatrixMath::dot(MatrixMath::Transpose(R2),R2)); |
cocorlow | 0:de56252b419e | 192 | Matrix R3(3,1); |
cocorlow | 0:de56252b419e | 193 | R3 << V1.getNumber( 2, 1 )*R2.getNumber( 3, 1 )-V1.getNumber( 3, 1 )*R2.getNumber( 2, 1 ) << V1.getNumber( 3, 1 )*R2.getNumber( 1, 1 )-V1.getNumber( 1, 1 )*R2.getNumber( 3, 1 ) << V1.getNumber( 1, 1 )*R2.getNumber( 2, 1 )-V1.getNumber( 2, 1 )*R2.getNumber( 1, 1 ); |
cocorlow | 0:de56252b419e | 194 | R3 *= 1.0/sqrt(MatrixMath::dot(MatrixMath::Transpose(R3),R3)); |
cocorlow | 0:de56252b419e | 195 | |
cocorlow | 0:de56252b419e | 196 | Matrix Mou(3,3); |
cocorlow | 0:de56252b419e | 197 | Mou << W1.getNumber( 1, 1 ) << Ou2.getNumber( 1, 1 ) << Ou3.getNumber( 1, 1 ) |
cocorlow | 0:de56252b419e | 198 | << W1.getNumber( 2, 1 ) << Ou2.getNumber( 2, 1 ) << Ou3.getNumber( 2, 1 ) |
cocorlow | 0:de56252b419e | 199 | << W1.getNumber( 3, 1 ) << Ou2.getNumber( 3, 1 ) << Ou3.getNumber( 3, 1 ); |
cocorlow | 0:de56252b419e | 200 | Matrix Mr(3,3); |
cocorlow | 0:de56252b419e | 201 | Mr << V1.getNumber( 1, 1 ) << R2.getNumber( 1, 1 ) << R3.getNumber( 1, 1 ) |
cocorlow | 0:de56252b419e | 202 | << V1.getNumber( 2, 1 ) << R2.getNumber( 2, 1 ) << R3.getNumber( 2, 1 ) |
cocorlow | 0:de56252b419e | 203 | << V1.getNumber( 3, 1 ) << R2.getNumber( 3, 1 ) << R3.getNumber( 3, 1 ); |
cocorlow | 0:de56252b419e | 204 | |
cocorlow | 0:de56252b419e | 205 | Matrix Cbn = Mr*MatrixMath::Transpose(Mou); |
cocorlow | 0:de56252b419e | 206 | |
cocorlow | 0:de56252b419e | 207 | float sqtrp1 = sqrt(1.0+Cbn.getNumber( 1, 1 )+Cbn.getNumber( 2, 2 )+Cbn.getNumber( 3, 3 )); |
cocorlow | 0:de56252b419e | 208 | |
cocorlow | 0:de56252b419e | 209 | qhat.add(1,1,0.5*sqtrp1); |
cocorlow | 0:de56252b419e | 210 | qhat.add(2,1,-(Cbn.getNumber( 2, 3 )-Cbn.getNumber( 3, 2 ))/2.0/sqtrp1); |
cocorlow | 0:de56252b419e | 211 | qhat.add(3,1,-(Cbn.getNumber( 3, 1 )-Cbn.getNumber( 1, 3 ))/2.0/sqtrp1); |
cocorlow | 0:de56252b419e | 212 | qhat.add(4,1,-(Cbn.getNumber( 1, 2 )-Cbn.getNumber( 2, 1 ))/2.0/sqtrp1); |
cocorlow | 0:de56252b419e | 213 | |
cocorlow | 0:de56252b419e | 214 | float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qhat),qhat)); |
cocorlow | 0:de56252b419e | 215 | qhat *= (1.0f/ qnorm); |
cocorlow | 0:de56252b419e | 216 | |
cocorlow | 0:de56252b419e | 217 | qhat_gyro = qhat; |
cocorlow | 0:de56252b419e | 218 | |
cocorlow | 0:de56252b419e | 219 | float q0 = qhat.getNumber( 1, 1 ); |
cocorlow | 0:de56252b419e | 220 | float q1 = qhat.getNumber( 2, 1 ); |
cocorlow | 0:de56252b419e | 221 | float q2 = qhat.getNumber( 3, 1 ); |
cocorlow | 0:de56252b419e | 222 | float q3 = qhat.getNumber( 4, 1 ); |
cocorlow | 0:de56252b419e | 223 | |
cocorlow | 0:de56252b419e | 224 | D.add(1,1,q0*q0 + q1*q1 - q2*q2 - q3*q3); |
cocorlow | 0:de56252b419e | 225 | D.add(1,2,2*(q1*q2 + q0*q3)); |
cocorlow | 0:de56252b419e | 226 | D.add(1,3,2*(q1*q3 - q0*q2)); |
cocorlow | 0:de56252b419e | 227 | D.add(2,1,2*(q1*q2 - q0*q3)); |
cocorlow | 0:de56252b419e | 228 | D.add(2,2,q0*q0 - q1*q1 + q2*q2 - q3*q3); |
cocorlow | 0:de56252b419e | 229 | D.add(2,3,2*(q2*q3 + q0*q1)); |
cocorlow | 0:de56252b419e | 230 | D.add(3,1,2*(q1*q3 + q0*q2)); |
cocorlow | 0:de56252b419e | 231 | D.add(3,2,2*(q2*q3 - q0*q1)); |
cocorlow | 0:de56252b419e | 232 | D.add(3,3,q0*q0 - q1*q1 - q2*q2 + q3*q3); |
cocorlow | 0:de56252b419e | 233 | } |
cocorlow | 0:de56252b419e | 234 | |
cocorlow | 0:de56252b419e | 235 | Vector3 HAPS_EKF::calcMagRef(Vector3 m) |
cocorlow | 0:de56252b419e | 236 | { |
cocorlow | 0:de56252b419e | 237 | float _x, _y, _z; |
cocorlow | 0:de56252b419e | 238 | Matrix magvec(3,1); |
cocorlow | 0:de56252b419e | 239 | magvec << m.x << m.y << m.z; |
cocorlow | 0:de56252b419e | 240 | Matrix magnedvec = MatrixMath::Transpose(D)*magvec; |
cocorlow | 0:de56252b419e | 241 | _x = sqrt(magnedvec(1,1)*magnedvec(1,1)+magnedvec(2,1)*magnedvec(2,1)); |
cocorlow | 0:de56252b419e | 242 | _y = 0.0f; |
cocorlow | 0:de56252b419e | 243 | _z = magnedvec(3,1); |
cocorlow | 0:de56252b419e | 244 | return Vector3(_x, _y, _z); |
cocorlow | 0:de56252b419e | 245 | } |
cocorlow | 0:de56252b419e | 246 | |
cocorlow | 0:de56252b419e | 247 | Vector3 HAPS_EKF::calcDynAcc(Vector3 LPacc, Vector3 accref) |
cocorlow | 0:de56252b419e | 248 | { |
cocorlow | 0:de56252b419e | 249 | float _x, _y, _z; |
cocorlow | 0:de56252b419e | 250 | _x = LPacc.x-(D.getNumber( 1, 1 )*accref.x+D.getNumber( 1, 2 )*accref.y+D.getNumber( 1, 3 )*accref.z); |
cocorlow | 0:de56252b419e | 251 | _y = LPacc.y-(D.getNumber( 2, 1 )*accref.x+D.getNumber( 2, 2 )*accref.y+D.getNumber( 2, 3 )*accref.z); |
cocorlow | 0:de56252b419e | 252 | _z = LPacc.z-(D.getNumber( 3, 1 )*accref.x+D.getNumber( 3, 2 )*accref.y+D.getNumber( 3, 3 )*accref.z); |
cocorlow | 0:de56252b419e | 253 | return Vector3(_x, _y, _z); |
cocorlow | 0:de56252b419e | 254 | } |