HAPSRG / solaESKF_Eigen

Eigen

Dependencies:   Eigen

Dependents:   optWingforHAPS_Eigen hexaTest_Eigen

ScErrStateEKF.cpp@20:37d3c3ee36e9, 2021-08-07 (annotated)

Committer:
NaotoMorita
Date:
Sat Aug 07 06:54:50 2021 +0000
Revision:
20:37d3c3ee36e9
Parent:
19:3fae66745363
Child:
21:d6079def0473
async update

Who changed what in which revision?

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