HAPSRG / solaESKF_Eigen

Eigen

Dependencies:   Eigen

Dependents:   optWingforHAPS_Eigen hexaTest_Eigen

ScErrStateEKF.cpp@38:1f6532849c05, 2021-10-10 (annotated)

Committer:
osaka
Date:
Sun Oct 10 09:15:29 2021 +0000
Revision:
38:1f6532849c05
Parent:
34:dec4b37db3f1
Child:
39:6834e05d8a64
(12, 12) matrix

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