HAPSRG / solaESKF_Eigen

Eigen

Dependencies:   Eigen

Dependents:   optWingforHAPS_Eigen hexaTest_Eigen

ScErrStateEKF.cpp@40:119792aa6d3b, 2021-10-18 (annotated)

Committer:
osaka
Date:
Mon Oct 18 12:16:57 2021 +0000
Revision:
40:119792aa6d3b
Parent:
39:6834e05d8a64
Child:
42:b90c348a49c0
Child:
43:cbc2c2d65131
errState with accBias

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