HAPSRG / solaESKF_Eigen

Eigen

Dependencies:   Eigen

Dependents:   optWingforHAPS_Eigen hexaTest_Eigen

ScErrStateEKF.cpp@25:07ac5c6cd61c, 2021-08-25 (annotated)

Committer:
NaotoMorita
Date:
Wed Aug 25 07:01:05 2021 +0000
Revision:
25:07ac5c6cd61c
Parent:
23:1509648c2318
Child:
26:73c3f58b9d70
centered correction

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