HAPSRG / solaESKF_Eigen

Eigen

Dependencies:   Eigen

Dependents:   optWingforHAPS_Eigen hexaTest_Eigen

ScErrStateEKF.cpp@42:b90c348a49c0, 2021-10-21 (annotated)

Committer:
osaka
Date:
Thu Oct 21 06:40:38 2021 +0000
Revision:
42:b90c348a49c0
Parent:
40:119792aa6d3b 
newer

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