HAPSRG / solaESKF_Eigen

Eigen

Dependencies:   Eigen

Dependents:   optWingforHAPS_Eigen hexaTest_Eigen

ScErrStateEKF.cpp@29:05e35ca2c9c4, 2021-09-16 (annotated)

Committer:
NaotoMorita
Date:
Thu Sep 16 09:30:14 2021 +0000
Revision:
29:05e35ca2c9c4
Parent:
27:a74f101ba40a
Child:
30:ff884e9b2e30
velocity fuse

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