HAPSRG / solaESKF_Eigen

Eigen

Dependencies:   Eigen

Dependents:   optWingforHAPS_Eigen hexaTest_Eigen

ScErrStateEKF.cpp@23:1509648c2318, 2021-08-20 (annotated)

Committer:
NaotoMorita
Date:
Fri Aug 20 07:48:35 2021 +0000
Revision:
23:1509648c2318
Parent:
22:7d84b8bc20b4
Child:
25:07ac5c6cd61c
20210819

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 19:3fae66745363 132
NaotoMorita 19:3fae66745363 133 void ScErrStateEKF::updateAccMeasures(Vector3 acc, Vector3 accref)
NaotoMorita 19:3fae66745363 134 {
NaotoMorita 21:d6079def0473 135 acc -= accBias;
NaotoMorita 20:37d3c3ee36e9 136 Matrix dcm(3,3);
NaotoMorita 20:37d3c3ee36e9 137 computeDcm(dcm, qhat);
NaotoMorita 20:37d3c3ee36e9 138 Vector3 gvec(dcm(1,3)*accref.z, dcm(2,3)*accref.z, dcm(3,3)*accref.z);
NaotoMorita 21:d6079def0473 139 Matrix H(3,nState);
NaotoMorita 20:37d3c3ee36e9 140 H(1,2) = -2.0f*gvec.z;
NaotoMorita 20:37d3c3ee36e9 141 H(1,3) = 2.0f*gvec.y;
NaotoMorita 20:37d3c3ee36e9 142 H(2,1) = 2.0f*gvec.z;
NaotoMorita 20:37d3c3ee36e9 143 H(2,3) = -2.0f*gvec.x;
NaotoMorita 20:37d3c3ee36e9 144 H(3,1) = -2.0f*gvec.y;
NaotoMorita 20:37d3c3ee36e9 145 H(3,2) = 2.0f*gvec.x;
NaotoMorita 20:37d3c3ee36e9 146 H(1,7) = 1.0f;
NaotoMorita 20:37d3c3ee36e9 147 H(2,8) = 1.0f;
NaotoMorita 20:37d3c3ee36e9 148 H(3,9) = 1.0f;
NaotoMorita 20:37d3c3ee36e9 149 Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+Ra+Qab);
NaotoMorita 21:d6079def0473 150 Matrix z = MatrixMath::Vector2mat(acc)-dcm*MatrixMath::Vector2mat(accref);
NaotoMorita 20:37d3c3ee36e9 151 Matrix corrVal = K * (z-H*errState);
NaotoMorita 20:37d3c3ee36e9 152 errState = errState + corrVal;
NaotoMorita 21:d6079def0473 153 Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*(Ra+Qab)*MatrixMath::Transpose(K);
NaotoMorita 20:37d3c3ee36e9 154 }
NaotoMorita 20:37d3c3ee36e9 155
NaotoMorita 20:37d3c3ee36e9 156 void ScErrStateEKF::updateGyroBiasConstraints(Vector3 gyro)
NaotoMorita 20:37d3c3ee36e9 157 {
NaotoMorita 21:d6079def0473 158 gyro -= gyroBias;
NaotoMorita 21:d6079def0473 159
NaotoMorita 20:37d3c3ee36e9 160 Matrix dcm(3,3);
NaotoMorita 20:37d3c3ee36e9 161 computeDcm(dcm, qhat);
NaotoMorita 23:1509648c2318 162 Matrix qeterm = -2.0f*MatrixMath::Transpose(dcm)*MatrixMath::Matrixcross(gyro.x,gyro.y,gyro.z);
NaotoMorita 23:1509648c2318 163 Matrix H(2,nState);
NaotoMorita 23:1509648c2318 164 for(int i = 1; i<3; i++){
NaotoMorita 23:1509648c2318 165 for(int j = 1;j<4;j++){
NaotoMorita 23:1509648c2318 166 H(i,j) = qeterm(i,j);
NaotoMorita 23:1509648c2318 167 }
NaotoMorita 23:1509648c2318 168 }
NaotoMorita 23:1509648c2318 169 H(1,4) = 1.0f*(dcm(1,1));
NaotoMorita 23:1509648c2318 170 H(1,5) = 1.0f*(dcm(2,1));
NaotoMorita 23:1509648c2318 171 H(1,6) = 1.0f*(dcm(3,1));
NaotoMorita 23:1509648c2318 172 H(2,4) = 1.0f*(dcm(1,2));
NaotoMorita 23:1509648c2318 173 H(2,5) = 1.0f*(dcm(2,2));
NaotoMorita 23:1509648c2318 174 H(2,6) = 1.0f*(dcm(3,2));
NaotoMorita 23:1509648c2318 175
NaotoMorita 23:1509648c2318 176 Matrix Pm(nState,nState);
NaotoMorita 23:1509648c2318 177 for(int i = 4; i<7; i++){
NaotoMorita 23:1509648c2318 178 for(int j = 4;j<7;j++){
NaotoMorita 23:1509648c2318 179 Pm(i,j) = Phat(i,j);
NaotoMorita 23:1509648c2318 180 }
NaotoMorita 23:1509648c2318 181 }
NaotoMorita 23:1509648c2318 182
NaotoMorita 23:1509648c2318 183 Matrix K = (Pm*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Pm*MatrixMath::Transpose(H)+Rgsc);
NaotoMorita 23:1509648c2318 184 Matrix z(2,1);
NaotoMorita 23:1509648c2318 185 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 186 Matrix corrVal = K * (z-H*errState);
NaotoMorita 23:1509648c2318 187 errState = errState + corrVal;
NaotoMorita 23:1509648c2318 188 Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*(Rgsc)*MatrixMath::Transpose(K);
NaotoMorita 23:1509648c2318 189 }
NaotoMorita 23:1509648c2318 190
NaotoMorita 23:1509648c2318 191 void ScErrStateEKF::updateVelocityConstraints()
NaotoMorita 23:1509648c2318 192 {
NaotoMorita 23:1509648c2318 193
NaotoMorita 23:1509648c2318 194 Matrix dcm(3,3);
NaotoMorita 23:1509648c2318 195 computeDcm(dcm, qhat);
NaotoMorita 21:d6079def0473 196 Matrix H(2,nState);
NaotoMorita 23:1509648c2318 197
NaotoMorita 23:1509648c2318 198 Matrix nomVb = dcm*vihat;
NaotoMorita 23:1509648c2318 199 Matrix qeterm = -2.0f*MatrixMath::Matrixcross(nomVb(1,1),nomVb(2,1),nomVb(3,1));
NaotoMorita 23:1509648c2318 200 for(int j = 1;j<4;j++){
NaotoMorita 23:1509648c2318 201 H(1,j) = -qeterm(1,j);
NaotoMorita 23:1509648c2318 202 H(2,j) = -qeterm(3,j);
NaotoMorita 23:1509648c2318 203 //H(3,j) = -qeterm(3,j);
NaotoMorita 23:1509648c2318 204 H(1,9+j) = -dcm(1,j);
NaotoMorita 23:1509648c2318 205 H(2,9+j) = -dcm(3,j);
NaotoMorita 23:1509648c2318 206 //H(3,9+j) = -dcm(3,j);
NaotoMorita 23:1509648c2318 207 }
NaotoMorita 23:1509648c2318 208
NaotoMorita 23:1509648c2318 209 Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+Rvsc);
NaotoMorita 23:1509648c2318 210 Matrix z(2,1);
NaotoMorita 23:1509648c2318 211 z << nomVb(1,1) << nomVb(3,1) ;
NaotoMorita 23:1509648c2318 212 Matrix corrVal = K * (z-H*errState);
NaotoMorita 23:1509648c2318 213 errState = errState + corrVal;
NaotoMorita 23:1509648c2318 214 Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*(Rvsc)*MatrixMath::Transpose(K);
NaotoMorita 23:1509648c2318 215
NaotoMorita 23:1509648c2318 216 }
NaotoMorita 23:1509648c2318 217
NaotoMorita 23:1509648c2318 218 void ScErrStateEKF::updateMeasures(Vector3 gyro, Vector3 acc, Vector3 accref)
NaotoMorita 23:1509648c2318 219 {
NaotoMorita 23:1509648c2318 220 acc -= accBias;
NaotoMorita 23:1509648c2318 221 gyro -= gyroBias;
NaotoMorita 23:1509648c2318 222 Matrix dcm(3,3);
NaotoMorita 23:1509648c2318 223 computeDcm(dcm, qhat);
NaotoMorita 23:1509648c2318 224 Vector3 gvec(dcm(1,3)*accref.z, dcm(2,3)*accref.z, dcm(3,3)*accref.z);
NaotoMorita 23:1509648c2318 225 Matrix H(5,nState);
NaotoMorita 23:1509648c2318 226 H(1,2) = -2.0f*gvec.z;
NaotoMorita 23:1509648c2318 227 H(1,3) = 2.0f*gvec.y;
NaotoMorita 23:1509648c2318 228 H(2,1) = 2.0f*gvec.z;
NaotoMorita 23:1509648c2318 229 H(2,3) = -2.0f*gvec.x;
NaotoMorita 23:1509648c2318 230 H(3,1) = -2.0f*gvec.y;
NaotoMorita 23:1509648c2318 231 H(3,2) = 2.0f*gvec.x;
NaotoMorita 23:1509648c2318 232 H(1,7) = 1.0f;
NaotoMorita 23:1509648c2318 233 H(2,8) = 1.0f;
NaotoMorita 23:1509648c2318 234 H(3,9) = 1.0f;
NaotoMorita 23:1509648c2318 235 /*
NaotoMorita 23:1509648c2318 236 Matrix qeterm = -2.0f*MatrixMath::Transpose(dcm)*MatrixMath::Matrixcross(gyro.x,gyro.y,gyro.z);
NaotoMorita 23:1509648c2318 237 for(int j = 1;j<4;j++){
NaotoMorita 23:1509648c2318 238 H(4,j) = qeterm(1,j);
NaotoMorita 23:1509648c2318 239 H(5,j) = qeterm(2,j);
NaotoMorita 23:1509648c2318 240 }
NaotoMorita 23:1509648c2318 241 H(4,4) = 1.0f*(dcm(1,1));
NaotoMorita 23:1509648c2318 242 H(4,5) = 1.0f*(dcm(2,1));
NaotoMorita 23:1509648c2318 243 H(4,6) = 1.0f*(dcm(3,1));
NaotoMorita 23:1509648c2318 244 H(5,4) = 1.0f*(dcm(1,2));
NaotoMorita 23:1509648c2318 245 H(5,5) = 1.0f*(dcm(2,2));
NaotoMorita 23:1509648c2318 246 H(5,6) = 1.0f*(dcm(3,2));
NaotoMorita 23:1509648c2318 247 */
NaotoMorita 23:1509648c2318 248 Matrix nomVb = dcm*vihat;
NaotoMorita 23:1509648c2318 249 Matrix qeterm = 2.0f*MatrixMath::Matrixcross(nomVb(1,1),nomVb(2,1),nomVb(3,1));
NaotoMorita 23:1509648c2318 250 for(int j = 1;j<4;j++){
NaotoMorita 23:1509648c2318 251 H(4,j) = -qeterm(1,j);
NaotoMorita 23:1509648c2318 252 H(5,j) = -qeterm(3,j);
NaotoMorita 23:1509648c2318 253 //H(3,j) = -qeterm(3,j);
NaotoMorita 23:1509648c2318 254 H(4,9+j) = -dcm(1,j);
NaotoMorita 23:1509648c2318 255 H(5,9+j) = -dcm(3,j);
NaotoMorita 23:1509648c2318 256 //H(3,9+j) = -dcm(3,j);
NaotoMorita 23:1509648c2318 257 }
NaotoMorita 23:1509648c2318 258
NaotoMorita 23:1509648c2318 259 Matrix R(5,5);
NaotoMorita 23:1509648c2318 260 R(1,1) = Ra(1,1)+Qab(1,1);
NaotoMorita 23:1509648c2318 261 R(2,2) = Ra(2,2)+Qab(2,2);
NaotoMorita 23:1509648c2318 262 R(3,3) = Ra(3,3)+Qab(3,3);
NaotoMorita 23:1509648c2318 263 //R(4,4) = Rgsc(1,1);
NaotoMorita 23:1509648c2318 264 //R(5,5) = Rgsc(2,2);
NaotoMorita 23:1509648c2318 265 R(4,4) = Rvsc(1,1);
NaotoMorita 23:1509648c2318 266 R(5,5) = Rvsc(2,2);
NaotoMorita 23:1509648c2318 267
NaotoMorita 23:1509648c2318 268 Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
NaotoMorita 23:1509648c2318 269 Matrix zacc = MatrixMath::Vector2mat(acc)-dcm*MatrixMath::Vector2mat(accref);
NaotoMorita 23:1509648c2318 270 Matrix z(5,1);
NaotoMorita 23:1509648c2318 271 z << zacc(1,1)<< zacc(2,1)<< zacc(3,1) << nomVb(1,1) << nomVb(3,1) ;
NaotoMorita 23:1509648c2318 272 Matrix corrVal = K * (z-H*errState);
NaotoMorita 23:1509648c2318 273 errState = errState + corrVal;
NaotoMorita 23:1509648c2318 274 Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*(R)*MatrixMath::Transpose(K);
NaotoMorita 23:1509648c2318 275 }
NaotoMorita 23:1509648c2318 276
NaotoMorita 23:1509648c2318 277
NaotoMorita 23:1509648c2318 278 void ScErrStateEKF::updateStaticMeasures(Vector3 gyro,Vector3 acc, Vector3 accref)
NaotoMorita 23:1509648c2318 279 {
NaotoMorita 23:1509648c2318 280 acc -= accBias;
NaotoMorita 23:1509648c2318 281 gyro -= gyroBias;
NaotoMorita 23:1509648c2318 282 Matrix dcm(3,3);
NaotoMorita 23:1509648c2318 283 computeDcm(dcm, qhat);
NaotoMorita 23:1509648c2318 284 Vector3 gvec(dcm(1,3)*accref.z, dcm(2,3)*accref.z, dcm(3,3)*accref.z);
NaotoMorita 23:1509648c2318 285 Matrix H(5,nState);
NaotoMorita 23:1509648c2318 286 H(1,2) = -2.0f*gvec.z;
NaotoMorita 23:1509648c2318 287 H(1,3) = 2.0f*gvec.y;
NaotoMorita 23:1509648c2318 288 H(2,1) = 2.0f*gvec.z;
NaotoMorita 23:1509648c2318 289 H(2,3) = -2.0f*gvec.x;
NaotoMorita 23:1509648c2318 290 H(3,1) = -2.0f*gvec.y;
NaotoMorita 23:1509648c2318 291 H(3,2) = 2.0f*gvec.x;
NaotoMorita 23:1509648c2318 292 H(1,7) = 1.0f;
NaotoMorita 23:1509648c2318 293 H(2,8) = 1.0f;
NaotoMorita 23:1509648c2318 294 H(3,9) = 1.0f;
NaotoMorita 23:1509648c2318 295 /*
NaotoMorita 23:1509648c2318 296 Matrix qeterm = -2.0f*MatrixMath::Transpose(dcm)*MatrixMath::Matrixcross(gyro.x,gyro.y,gyro.z);
NaotoMorita 23:1509648c2318 297 for(int j = 1;j<4;j++){
NaotoMorita 23:1509648c2318 298 H(4,j) = qeterm(1,j);
NaotoMorita 23:1509648c2318 299 H(5,j) = qeterm(2,j);
NaotoMorita 23:1509648c2318 300 }
NaotoMorita 23:1509648c2318 301 H(4,4) = 1.0f*(dcm(1,1));
NaotoMorita 23:1509648c2318 302 H(4,5) = 1.0f*(dcm(2,1));
NaotoMorita 23:1509648c2318 303 H(4,6) = 1.0f*(dcm(3,1));
NaotoMorita 23:1509648c2318 304 H(5,4) = 1.0f*(dcm(1,2));
NaotoMorita 23:1509648c2318 305 H(5,5) = 1.0f*(dcm(2,2));
NaotoMorita 23:1509648c2318 306 H(5,6) = 1.0f*(dcm(3,2));
NaotoMorita 23:1509648c2318 307 */
NaotoMorita 23:1509648c2318 308 Matrix nomVb = dcm*vihat;
NaotoMorita 23:1509648c2318 309 Matrix qeterm = 2.0f*MatrixMath::Matrixcross(nomVb(1,1),nomVb(2,1),nomVb(3,1));
NaotoMorita 23:1509648c2318 310 for(int j = 1;j<4;j++){
NaotoMorita 23:1509648c2318 311 H(4,j) = -qeterm(1,j);
NaotoMorita 23:1509648c2318 312 H(5,j) = -qeterm(3,j);
NaotoMorita 23:1509648c2318 313 //H(3,j) = -qeterm(3,j);
NaotoMorita 23:1509648c2318 314 H(4,9+j) = -dcm(1,j);
NaotoMorita 23:1509648c2318 315 H(5,9+j) = -dcm(3,j);
NaotoMorita 23:1509648c2318 316 //H(3,9+j) = -dcm(3,j);
NaotoMorita 23:1509648c2318 317 }
NaotoMorita 23:1509648c2318 318
NaotoMorita 23:1509648c2318 319 Matrix R(5,5);
NaotoMorita 23:1509648c2318 320 R(1,1) = Ra(1,1)*100.0f;
NaotoMorita 23:1509648c2318 321 R(2,2) = Ra(2,2)*100.0f;
NaotoMorita 23:1509648c2318 322 R(3,3) = Ra(3,3)*100.0f;
NaotoMorita 23:1509648c2318 323 //R(4,4) = Rgsc(1,1);
NaotoMorita 23:1509648c2318 324 //R(5,5) = Rgsc(2,2);
NaotoMorita 23:1509648c2318 325 R(4,4) = Rvsc(1,1);
NaotoMorita 23:1509648c2318 326 R(5,5) = Rvsc(2,2);
NaotoMorita 23:1509648c2318 327
NaotoMorita 23:1509648c2318 328 Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
NaotoMorita 23:1509648c2318 329 Matrix zacc = MatrixMath::Vector2mat(acc)-dcm*MatrixMath::Vector2mat(accref);
NaotoMorita 23:1509648c2318 330 Matrix z(5,1);
NaotoMorita 23:1509648c2318 331 z << zacc(1,1)<< zacc(2,1)<< zacc(3,1) << nomVb(1,1) << nomVb(3,1) ;
NaotoMorita 23:1509648c2318 332 Matrix corrVal = K * (z-H*errState);
NaotoMorita 23:1509648c2318 333 errState = errState + corrVal;
NaotoMorita 23:1509648c2318 334 Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*(R)*MatrixMath::Transpose(K);
NaotoMorita 23:1509648c2318 335 }
NaotoMorita 23:1509648c2318 336
NaotoMorita 23:1509648c2318 337 void ScErrStateEKF::updateConstraints(Vector3 gyro)
NaotoMorita 23:1509648c2318 338 {
NaotoMorita 23:1509648c2318 339
NaotoMorita 23:1509648c2318 340 gyro -=gyroBias;
NaotoMorita 23:1509648c2318 341
NaotoMorita 23:1509648c2318 342 Matrix dcm(3,3);
NaotoMorita 23:1509648c2318 343 computeDcm(dcm, qhat);
NaotoMorita 23:1509648c2318 344
NaotoMorita 23:1509648c2318 345 Matrix qeterm = -2.0f*MatrixMath::Transpose(dcm)*MatrixMath::Matrixcross(gyro.x,gyro.y,gyro.z);
NaotoMorita 23:1509648c2318 346 Matrix H(4,nState);
NaotoMorita 21:d6079def0473 347 for(int i = 1; i<3; i++){
NaotoMorita 21:d6079def0473 348 for(int j = 1;j<4;j++){
NaotoMorita 21:d6079def0473 349 H(i,j) = qeterm(i,j);
NaotoMorita 21:d6079def0473 350 }
NaotoMorita 21:d6079def0473 351 }
NaotoMorita 21:d6079def0473 352 H(1,4) = 1.0f*(dcm(1,1));
NaotoMorita 21:d6079def0473 353 H(1,5) = 1.0f*(dcm(2,1));
NaotoMorita 21:d6079def0473 354 H(1,6) = 1.0f*(dcm(3,1));
NaotoMorita 21:d6079def0473 355 H(2,4) = 1.0f*(dcm(1,2));
NaotoMorita 21:d6079def0473 356 H(2,5) = 1.0f*(dcm(2,2));
NaotoMorita 21:d6079def0473 357 H(2,6) = 1.0f*(dcm(3,2));
NaotoMorita 23:1509648c2318 358
NaotoMorita 23:1509648c2318 359 Matrix nomVb = dcm*vihat;
NaotoMorita 23:1509648c2318 360 qeterm = -2.0f*MatrixMath::Matrixcross(nomVb(1,1),nomVb(2,1),nomVb(3,1));
NaotoMorita 23:1509648c2318 361 for(int j = 1;j<4;j++){
NaotoMorita 23:1509648c2318 362 H(3,j) = -qeterm(1,j);
NaotoMorita 23:1509648c2318 363 H(4,j) = -qeterm(3,j);
NaotoMorita 23:1509648c2318 364 //H(3,j) = -qeterm(3,j);
NaotoMorita 23:1509648c2318 365 H(3,9+j) = -dcm(1,j);
NaotoMorita 23:1509648c2318 366 H(4,9+j) = -dcm(3,j);
NaotoMorita 23:1509648c2318 367 //H(3,9+j) = -dcm(3,j);
NaotoMorita 23:1509648c2318 368 }
NaotoMorita 23:1509648c2318 369
NaotoMorita 23:1509648c2318 370
NaotoMorita 23:1509648c2318 371 Matrix R(4,4);
NaotoMorita 23:1509648c2318 372 R(1,1) = Rgsc(1,1);
NaotoMorita 23:1509648c2318 373 R(2,2) = Rgsc(2,1);
NaotoMorita 23:1509648c2318 374 R(3,3) = Rvsc(1,1);
NaotoMorita 23:1509648c2318 375 R(4,4) = Rvsc(2,1);
NaotoMorita 23:1509648c2318 376 //R(5,5) = Rvsc(3,1);
NaotoMorita 23:1509648c2318 377 Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
NaotoMorita 23:1509648c2318 378 Matrix z(4,1);
NaotoMorita 23:1509648c2318 379 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 380 Matrix corrVal = K * (z-H*errState);
NaotoMorita 20:37d3c3ee36e9 381 errState = errState + corrVal;
NaotoMorita 23:1509648c2318 382 Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*(R)*MatrixMath::Transpose(K);
NaotoMorita 23:1509648c2318 383 }
NaotoMorita 21:d6079def0473 384
NaotoMorita 21:d6079def0473 385
NaotoMorita 19:3fae66745363 386 void ScErrStateEKF::updateStaticAccMeasures(Vector3 acc, Vector3 accref)
NaotoMorita 19:3fae66745363 387 {
NaotoMorita 19:3fae66745363 388 Matrix dcm(3,3);
NaotoMorita 19:3fae66745363 389 computeDcm(dcm, qhat);
NaotoMorita 19:3fae66745363 390 Vector3 gvec(dcm(1,3)*accref.z, dcm(2,3)*accref.z, dcm(3,3)*accref.z);
NaotoMorita 21:d6079def0473 391 Matrix H(3,nState);
NaotoMorita 19:3fae66745363 392 H(1,2) = -2.0f*gvec.z;
NaotoMorita 19:3fae66745363 393 H(1,3) = 2.0f*gvec.y;
NaotoMorita 19:3fae66745363 394 H(2,1) = 2.0f*gvec.z;
NaotoMorita 19:3fae66745363 395 H(2,3) = -2.0f*gvec.x;
NaotoMorita 19:3fae66745363 396 H(3,1) = -2.0f*gvec.y;
NaotoMorita 19:3fae66745363 397 H(3,2) = 2.0f*gvec.x;
NaotoMorita 19:3fae66745363 398 Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+Ra*100.0f);
NaotoMorita 19:3fae66745363 399 Matrix accmat(3,1);
NaotoMorita 19:3fae66745363 400 accmat << acc.x << acc.y << acc.z;
NaotoMorita 19:3fae66745363 401 Matrix gref(3,1);
NaotoMorita 19:3fae66745363 402 gref << 0.0f << 0.0f << accref.z;
NaotoMorita 19:3fae66745363 403 Matrix z = accmat-dcm*gref;
NaotoMorita 19:3fae66745363 404 errState = errState + K * (z-H*errState);
NaotoMorita 21:d6079def0473 405 Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*(Ra*100.0f)*MatrixMath::Transpose(K);
NaotoMorita 19:3fae66745363 406 }
NaotoMorita 19:3fae66745363 407
NaotoMorita 19:3fae66745363 408 void ScErrStateEKF::updateMagMeasures(Vector3 mag)
NaotoMorita 19:3fae66745363 409 {
NaotoMorita 19:3fae66745363 410 //mag = mag/mag.Norm();
NaotoMorita 19:3fae66745363 411 Matrix dcm(3,3);
NaotoMorita 19:3fae66745363 412 computeDcm(dcm, qhat);
NaotoMorita 19:3fae66745363 413
NaotoMorita 19:3fae66745363 414 Matrix magvec(3,1);
NaotoMorita 19:3fae66745363 415 magvec(1,1) = mag.x;
NaotoMorita 19:3fae66745363 416 magvec(2,1) = mag.y;
NaotoMorita 19:3fae66745363 417 magvec(3,1) = mag.z;
NaotoMorita 19:3fae66745363 418
NaotoMorita 19:3fae66745363 419 Matrix magnedvec = MatrixMath::Transpose(dcm)*magvec;
NaotoMorita 19:3fae66745363 420 Matrix magrefmod(3,1);
NaotoMorita 19:3fae66745363 421 magrefmod(1,1) = sqrt(magnedvec(1,1)*magnedvec(1,1)+magnedvec(2,1)*magnedvec(2,1));
NaotoMorita 19:3fae66745363 422 magrefmod(2,1) = 0.0f;
NaotoMorita 19:3fae66745363 423 magrefmod(3,1) = magnedvec(3,1);
NaotoMorita 19:3fae66745363 424
NaotoMorita 19:3fae66745363 425 Matrix mvec = dcm*magrefmod ;
NaotoMorita 21:d6079def0473 426 Matrix H(3,nState);
NaotoMorita 19:3fae66745363 427 H(1,2) = -2.0f*magvec(3,1);
NaotoMorita 19:3fae66745363 428 H(1,3) = 2.0f*magvec(2,1);
NaotoMorita 19:3fae66745363 429 H(2,1) = 2.0f*magvec(3,1);
NaotoMorita 19:3fae66745363 430 H(2,3) = -2.0f*magvec(1,1);
NaotoMorita 19:3fae66745363 431 H(3,1) = -2.0f*magvec(2,1);
NaotoMorita 19:3fae66745363 432 H(3,2) = 2.0f*magvec(1,1);
NaotoMorita 21:d6079def0473 433 Matrix Pm(nState,nState);
NaotoMorita 19:3fae66745363 434 for(int i = 1; i<4; i++){
NaotoMorita 19:3fae66745363 435 for(int j = 1;j<4;j++){
NaotoMorita 19:3fae66745363 436 Pm(i,j) = Phat(i,j);
NaotoMorita 19:3fae66745363 437 }
NaotoMorita 19:3fae66745363 438 }
NaotoMorita 19:3fae66745363 439 Matrix r3(3,1);
NaotoMorita 19:3fae66745363 440 r3 << dcm(1,3)<< dcm(2,3) << dcm(3,3);
NaotoMorita 19:3fae66745363 441 Matrix kpart = r3*MatrixMath::Transpose(r3);
NaotoMorita 21:d6079def0473 442 Matrix Kmod(nState,nState);
NaotoMorita 19:3fae66745363 443 for(int i = 1; i<4; i++){
NaotoMorita 19:3fae66745363 444 for(int j = 1;j<4;j++){
NaotoMorita 19:3fae66745363 445 Kmod(i,j) = kpart(i,j);
NaotoMorita 19:3fae66745363 446 }
NaotoMorita 19:3fae66745363 447 }
NaotoMorita 19:3fae66745363 448
NaotoMorita 19:3fae66745363 449 Matrix K = Kmod*(Pm*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Pm*MatrixMath::Transpose(H)+Rm);
NaotoMorita 19:3fae66745363 450 Matrix z = magvec-mvec;
NaotoMorita 19:3fae66745363 451 errState = errState + K * (z-H*errState);
NaotoMorita 21:d6079def0473 452 Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*(Rm)*MatrixMath::Transpose(K);
NaotoMorita 19:3fae66745363 453 }
NaotoMorita 19:3fae66745363 454
NaotoMorita 19:3fae66745363 455 void ScErrStateEKF::resetBias()
NaotoMorita 19:3fae66745363 456 {
NaotoMorita 19:3fae66745363 457 gyroBias.x = gyroBias.x + errState(4,1)*1.0f;
NaotoMorita 19:3fae66745363 458 gyroBias.y = gyroBias.y + errState(5,1)*1.0f;
NaotoMorita 19:3fae66745363 459 gyroBias.z = gyroBias.z + errState(6,1)*1.0f;
NaotoMorita 19:3fae66745363 460 errState(4,1) = 0.0f;
NaotoMorita 19:3fae66745363 461 errState(5,1) = 0.0f;
NaotoMorita 19:3fae66745363 462 errState(6,1) = 0.0f;
NaotoMorita 20:37d3c3ee36e9 463 accBias.x = accBias.x + errState(7,1);
NaotoMorita 20:37d3c3ee36e9 464 accBias.y = accBias.y + errState(8,1);
NaotoMorita 20:37d3c3ee36e9 465 accBias.z = accBias.z + errState(9,1);
NaotoMorita 20:37d3c3ee36e9 466 errState(7,1) = 0.0f;
NaotoMorita 20:37d3c3ee36e9 467 errState(8,1) = 0.0f;
NaotoMorita 20:37d3c3ee36e9 468 errState(9,1) = 0.0f;
NaotoMorita 19:3fae66745363 469 }
NaotoMorita 19:3fae66745363 470
NaotoMorita 19:3fae66745363 471 void ScErrStateEKF::computeAngles(Vector3& rpy,Vector3 rpy_align)
NaotoMorita 19:3fae66745363 472 {
NaotoMorita 19:3fae66745363 473 Matrix qerr(4,1);
NaotoMorita 19:3fae66745363 474 qerr << 1.0f << errState(1,1) << errState(2,1) << errState(3,1);
NaotoMorita 19:3fae66745363 475 //float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qerr),qerr));
NaotoMorita 19:3fae66745363 476 //qerr *= (1.0f/ qnorm);
NaotoMorita 19:3fae66745363 477 Matrix qest = quatmultiply(qhat, qerr);
NaotoMorita 19:3fae66745363 478 float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qest),qest));
NaotoMorita 19:3fae66745363 479 qest *= (1.0f/ qnorm);
NaotoMorita 19:3fae66745363 480 float q0 = qest( 1, 1 );
NaotoMorita 19:3fae66745363 481 float q1 = qest( 2, 1 );
NaotoMorita 19:3fae66745363 482 float q2 = qest( 3, 1 );
NaotoMorita 19:3fae66745363 483 float q3 = qest( 4, 1 );
NaotoMorita 19:3fae66745363 484 rpy.x = atan2f(q0*q1 + q2*q3, 0.5f - q1*q1 - q2*q2)-rpy_align.x;
NaotoMorita 19:3fae66745363 485 rpy.y = asinf(-2.0f * (q1*q3 - q0*q2))-rpy_align.y;
NaotoMorita 19:3fae66745363 486 rpy.z = atan2f(q1*q2 + q0*q3, 0.5f - q2*q2 - q3*q3);
NaotoMorita 19:3fae66745363 487
NaotoMorita 19:3fae66745363 488 }
NaotoMorita 19:3fae66745363 489
NaotoMorita 21:d6079def0473 490
NaotoMorita 21:d6079def0473 491 void ScErrStateEKF::fuseErr2Nominal()
NaotoMorita 19:3fae66745363 492 {
NaotoMorita 19:3fae66745363 493 Matrix qerr(4,1);
NaotoMorita 19:3fae66745363 494 qerr << 1.0f << errState(1,1) << errState(2,1) << errState(3,1);
NaotoMorita 19:3fae66745363 495 //float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qerr),qerr));
NaotoMorita 19:3fae66745363 496 //qerr *= (1.0f/ qnorm);
NaotoMorita 19:3fae66745363 497 qhat = quatmultiply(qhat, qerr);
NaotoMorita 19:3fae66745363 498 errState(1,1) = 0.0f;
NaotoMorita 19:3fae66745363 499 errState(2,1) = 0.0f;
NaotoMorita 19:3fae66745363 500 errState(3,1) = 0.0f;
NaotoMorita 19:3fae66745363 501 float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qhat),qhat));
NaotoMorita 19:3fae66745363 502 qhat *= (1.0f/ qnorm);
NaotoMorita 23:1509648c2318 503
NaotoMorita 23:1509648c2318 504 vihat(1,1) += errState(10,1);
NaotoMorita 23:1509648c2318 505 vihat(2,1) += errState(11,1);
NaotoMorita 23:1509648c2318 506 vihat(3,1) += errState(12,1);
NaotoMorita 23:1509648c2318 507 errState(10,1) = 0.0f;
NaotoMorita 23:1509648c2318 508 errState(11,1) = 0.0f;
NaotoMorita 23:1509648c2318 509 errState(12,1) = 0.0f;
NaotoMorita 19:3fae66745363 510 }
NaotoMorita 19:3fae66745363 511
NaotoMorita 19:3fae66745363 512 Matrix ScErrStateEKF::quatmultiply(Matrix q, Matrix r)
NaotoMorita 19:3fae66745363 513 {
NaotoMorita 19:3fae66745363 514 Matrix qout(4,1);
NaotoMorita 19:3fae66745363 515 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 516 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 517 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 518 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 519 float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qout),qout));
NaotoMorita 19:3fae66745363 520 qout *= (1.0f/ qnorm);
NaotoMorita 19:3fae66745363 521 return qout;
NaotoMorita 19:3fae66745363 522 }
NaotoMorita 19:3fae66745363 523
NaotoMorita 19:3fae66745363 524 void ScErrStateEKF::computeDcm(Matrix& dcm, Matrix quat)
NaotoMorita 19:3fae66745363 525 {
NaotoMorita 19:3fae66745363 526
NaotoMorita 19:3fae66745363 527 float q0 = quat( 1, 1 );
NaotoMorita 19:3fae66745363 528 float q1 = quat( 2, 1 );
NaotoMorita 19:3fae66745363 529 float q2 = quat( 3, 1 );
NaotoMorita 19:3fae66745363 530 float q3 = quat( 4, 1 );
NaotoMorita 19:3fae66745363 531
NaotoMorita 19:3fae66745363 532 dcm(1,1) = q0*q0 + q1*q1 - q2*q2 - q3*q3;
NaotoMorita 19:3fae66745363 533 dcm(1,2) = 2*(q1*q2 + q0*q3);
NaotoMorita 19:3fae66745363 534 dcm(1,3) = 2*(q1*q3 - q0*q2);
NaotoMorita 19:3fae66745363 535 dcm(2,1) = 2*(q1*q2 - q0*q3);
NaotoMorita 19:3fae66745363 536 dcm(2,2) = q0*q0 - q1*q1 + q2*q2 - q3*q3;
NaotoMorita 19:3fae66745363 537 dcm(2,3) = 2*(q2*q3 + q0*q1);
NaotoMorita 19:3fae66745363 538 dcm(3,1) = 2*(q1*q3 + q0*q2);
NaotoMorita 19:3fae66745363 539 dcm(3,2) = 2*(q2*q3 - q0*q1);
NaotoMorita 19:3fae66745363 540 dcm(3,3) = q0*q0 - q1*q1 - q2*q2 + q3*q3;
NaotoMorita 19:3fae66745363 541
NaotoMorita 19:3fae66745363 542 }
NaotoMorita 19:3fae66745363 543
NaotoMorita 19:3fae66745363 544 void ScErrStateEKF::defineQhat(Vector3 align){
NaotoMorita 19:3fae66745363 545 float cos_z_2 = cosf(0.5f*align.z);
NaotoMorita 19:3fae66745363 546 float cos_y_2 = cosf(0.5f*align.y);
NaotoMorita 19:3fae66745363 547 float cos_x_2 = cosf(0.5f*align.x);
NaotoMorita 19:3fae66745363 548
NaotoMorita 19:3fae66745363 549 float sin_z_2 = sinf(0.5f*align.z);
NaotoMorita 19:3fae66745363 550 float sin_y_2 = sinf(0.5f*align.y);
NaotoMorita 19:3fae66745363 551 float sin_x_2 = sinf(0.5f*align.x);
NaotoMorita 19:3fae66745363 552
NaotoMorita 19:3fae66745363 553 // and now compute quaternion
NaotoMorita 19:3fae66745363 554 qhat(1,1) = cos_z_2*cos_y_2*cos_x_2 + sin_z_2*sin_y_2*sin_x_2;
NaotoMorita 19:3fae66745363 555 qhat(2,1) = cos_z_2*cos_y_2*sin_x_2 - sin_z_2*sin_y_2*cos_x_2;
NaotoMorita 19:3fae66745363 556 qhat(3,1) = cos_z_2*sin_y_2*cos_x_2 + sin_z_2*cos_y_2*sin_x_2;
NaotoMorita 19:3fae66745363 557 qhat(4,1) = sin_z_2*cos_y_2*cos_x_2 - cos_z_2*sin_y_2*sin_x_2;
NaotoMorita 19:3fae66745363 558 }
NaotoMorita 19:3fae66745363 559
NaotoMorita 23:1509648c2318 560 void ScErrStateEKF::computeVb(Vector3& vb)
NaotoMorita 23:1509648c2318 561 {
NaotoMorita 23:1509648c2318 562 Matrix dcm(3,3);
NaotoMorita 23:1509648c2318 563 computeDcm(dcm, qhat);
NaotoMorita 23:1509648c2318 564 Matrix vbmat = dcm*vihat;
NaotoMorita 23:1509648c2318 565 vb.x = vbmat(1,1);
NaotoMorita 23:1509648c2318 566 vb.y = vbmat(2,1);
NaotoMorita 23:1509648c2318 567 vb.z = vbmat(3,1);
NaotoMorita 23:1509648c2318 568
NaotoMorita 23:1509648c2318 569 }
NaotoMorita 23:1509648c2318 570
NaotoMorita 19:3fae66745363 571
NaotoMorita 19:3fae66745363 572
NaotoMorita 19:3fae66745363 573 Vector3 ScErrStateEKF::calcDynAcc(Vector3 acc, Vector3 accref)
NaotoMorita 19:3fae66745363 574 {
NaotoMorita 19:3fae66745363 575 Matrix dcm(3,3);
NaotoMorita 19:3fae66745363 576 computeDcm(dcm, qhat);
NaotoMorita 19:3fae66745363 577 float _x, _y, _z;
NaotoMorita 19:3fae66745363 578 _x = acc.x-(dcm( 1, 1 )*accref.x+dcm( 1, 2 )*accref.y+dcm( 1, 3 )*accref.z);
NaotoMorita 19:3fae66745363 579 _y = acc.y-(dcm( 2, 1 )*accref.x+dcm( 2, 2 )*accref.y+dcm( 2, 3 )*accref.z);
NaotoMorita 19:3fae66745363 580 _z = acc.z-(dcm( 3, 1 )*accref.x+dcm( 3, 2 )*accref.y+dcm( 3, 3 )*accref.z);
NaotoMorita 19:3fae66745363 581 return Vector3(_x, _y, _z);
NaotoMorita 19:3fae66745363 582 }
NaotoMorita 19:3fae66745363 583
NaotoMorita 19:3fae66745363 584
NaotoMorita 19:3fae66745363 585 bool ScErrStateEKF::determinDynStatus(Vector3 acc, Vector3 accref)
NaotoMorita 19:3fae66745363 586 {
NaotoMorita 19:3fae66745363 587 histffunc[histffuncindex] = acc.Norm()*acc.Norm()-accref.Norm()*accref.Norm()-3.0f*sigma2a;
NaotoMorita 19:3fae66745363 588 if(histffuncindex<9){
NaotoMorita 19:3fae66745363 589 histffuncindex += 1;
NaotoMorita 19:3fae66745363 590 }else{
NaotoMorita 19:3fae66745363 591 histffuncindex =0;
NaotoMorita 19:3fae66745363 592 }
NaotoMorita 19:3fae66745363 593 aveffunc = 0;
NaotoMorita 19:3fae66745363 594 for(int i = 1;i<10;i++){
NaotoMorita 19:3fae66745363 595 aveffunc += 1.0f/10.0f*histffunc[i];
NaotoMorita 19:3fae66745363 596 }
NaotoMorita 19:3fae66745363 597 sigma2f = 1.0f/10.0f*(6.0f*sigma2a*sigma2a+4.0f*accref.Norm()*accref.Norm()*sigma2a);
NaotoMorita 19:3fae66745363 598
NaotoMorita 19:3fae66745363 599
NaotoMorita 19:3fae66745363 600 dynacc = calcDynAcc(acc,accref);
NaotoMorita 19:3fae66745363 601 bool dynCase = true;
NaotoMorita 19:3fae66745363 602 if((dynacc.Norm()<0.005f) && (abs(acc.Norm()-accref.Norm())<0.005f)){dynCase = false;}
NaotoMorita 19:3fae66745363 603 if(aveffunc<sqrt(sigma2f/0.99f) && abs(acc.Norm()-accref.Norm())<0.001f){dynCase = false;}
NaotoMorita 19:3fae66745363 604 return dynCase;
NaotoMorita 19:3fae66745363 605
NaotoMorita 19:3fae66745363 606 }
NaotoMorita 19:3fae66745363 607
NaotoMorita 22:7d84b8bc20b4 608 /*
NaotoMorita 22:7d84b8bc20b4 609 void ScErrStateEKF::updateNominal(Vector3 gyro, Vector3 acc, Vector3 accref, float att_dt)
NaotoMorita 22:7d84b8bc20b4 610 {
NaotoMorita 22:7d84b8bc20b4 611 gyro -= gyroBias;
NaotoMorita 22:7d84b8bc20b4 612 acc -= accBias;
NaotoMorita 22:7d84b8bc20b4 613 Matrix A(4,4);
NaotoMorita 22:7d84b8bc20b4 614 A << 0.0f << -0.5f*gyro.x <<-0.5f*gyro.y <<-0.5f*gyro.z
NaotoMorita 22:7d84b8bc20b4 615 << 0.5f*gyro.x << 0.0f << 0.5f*gyro.z <<-0.5f*gyro.y
NaotoMorita 22:7d84b8bc20b4 616 << 0.5f*gyro.y << -0.5f*gyro.z << 0.0f << 0.5f*gyro.x
NaotoMorita 22:7d84b8bc20b4 617 << 0.5f*gyro.z << 0.5f*gyro.y <<-0.5f*gyro.x << 0.0f ;
NaotoMorita 22:7d84b8bc20b4 618
NaotoMorita 22:7d84b8bc20b4 619 Matrix phi = MatrixMath::Eye(4) + A * att_dt + (0.5f*att_dt*att_dt) * A * A;
NaotoMorita 22:7d84b8bc20b4 620 qhat = phi * qhat;
NaotoMorita 22:7d84b8bc20b4 621 float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qhat),qhat));
NaotoMorita 22:7d84b8bc20b4 622 qhat *= (1.0f/ qnorm);
NaotoMorita 22:7d84b8bc20b4 623
NaotoMorita 22:7d84b8bc20b4 624 Matrix dcm(3,3);
NaotoMorita 22:7d84b8bc20b4 625 computeDcm(dcm, qhat);
NaotoMorita 22:7d84b8bc20b4 626 vihat += (MatrixMath::Transpose(dcm)*MatrixMath::Vector2mat(acc)-MatrixMath::Vector2mat(accref))*att_dt*9.8f;
NaotoMorita 22:7d84b8bc20b4 627 }
NaotoMorita 19:3fae66745363 628
NaotoMorita 22:7d84b8bc20b4 629 void ScErrStateEKF::updateErrState(Vector3 gyro,Vector3 acc, float att_dt)
NaotoMorita 22:7d84b8bc20b4 630 {
NaotoMorita 22:7d84b8bc20b4 631 gyro -= gyroBias;
NaotoMorita 22:7d84b8bc20b4 632 acc -= accBias;
NaotoMorita 22:7d84b8bc20b4 633 Matrix A(nState,nState);
NaotoMorita 22:7d84b8bc20b4 634 A(1,2) = gyro.z;
NaotoMorita 22:7d84b8bc20b4 635 A(1,3) = -gyro.y;
NaotoMorita 22:7d84b8bc20b4 636 A(2,1) = -gyro.z;
NaotoMorita 22:7d84b8bc20b4 637 A(2,3) = gyro.x;
NaotoMorita 22:7d84b8bc20b4 638 A(3,1) = gyro.y;
NaotoMorita 22:7d84b8bc20b4 639 A(3,2) = -gyro.x;
NaotoMorita 22:7d84b8bc20b4 640 A(1,4) = -0.5f;
NaotoMorita 22:7d84b8bc20b4 641 A(2,5) = -0.5f;
NaotoMorita 22:7d84b8bc20b4 642 A(3,6) = -0.5f;
NaotoMorita 22:7d84b8bc20b4 643
NaotoMorita 22:7d84b8bc20b4 644 Matrix dcm(3,3);
NaotoMorita 22:7d84b8bc20b4 645 computeDcm(dcm, qhat);
NaotoMorita 22:7d84b8bc20b4 646 Matrix qeterm = -2.0f*9.8f*MatrixMath::Transpose(dcm)*MatrixMath::Matrixcross(acc.x,acc.y,acc.z);
NaotoMorita 22:7d84b8bc20b4 647 Matrix baterm = -9.8f*MatrixMath::Transpose(dcm);
NaotoMorita 22:7d84b8bc20b4 648 for (int i = 1; i < 4; i++){
NaotoMorita 22:7d84b8bc20b4 649 for (int j = 1; i < 4; i++){
NaotoMorita 22:7d84b8bc20b4 650 A(i+9,j) = qeterm(i,j);
NaotoMorita 22:7d84b8bc20b4 651 A(i+9,j+6) = baterm(i,j);
NaotoMorita 22:7d84b8bc20b4 652 }
NaotoMorita 22:7d84b8bc20b4 653 }
NaotoMorita 22:7d84b8bc20b4 654
NaotoMorita 22:7d84b8bc20b4 655 Matrix phi = MatrixMath::Eye(nState) + A * att_dt + (0.5f*att_dt*att_dt) * A * A;
NaotoMorita 22:7d84b8bc20b4 656 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 657 errState = phi * errState;
NaotoMorita 22:7d84b8bc20b4 658 Phat = phi*Phat*MatrixMath::Transpose(phi)+Qd;
NaotoMorita 22:7d84b8bc20b4 659 }
NaotoMorita 22:7d84b8bc20b4 660
NaotoMorita 22:7d84b8bc20b4 661 void ScErrStateEKF::updateVelocityConstraints()
NaotoMorita 22:7d84b8bc20b4 662 {
NaotoMorita 22:7d84b8bc20b4 663
NaotoMorita 22:7d84b8bc20b4 664 Matrix dcm(3,3);
NaotoMorita 22:7d84b8bc20b4 665 computeDcm(dcm, qhat);
NaotoMorita 22:7d84b8bc20b4 666 Matrix H(2,nState);
NaotoMorita 22:7d84b8bc20b4 667
NaotoMorita 22:7d84b8bc20b4 668 Matrix nomVb = dcm*vihat;
NaotoMorita 22:7d84b8bc20b4 669 Matrix qeterm = -2.0f*MatrixMath::Matrixcross(nomVb(1,1),nomVb(2,1),nomVb(3,1));
NaotoMorita 22:7d84b8bc20b4 670 for(int j = 1;j<4;j++){
NaotoMorita 22:7d84b8bc20b4 671 H(1,j) = -qeterm(1,j);
NaotoMorita 22:7d84b8bc20b4 672 H(2,j) = -qeterm(3,j);
NaotoMorita 22:7d84b8bc20b4 673 //H(3,j) = -qeterm(3,j);
NaotoMorita 22:7d84b8bc20b4 674 H(1,9+j) = -dcm(1,j);
NaotoMorita 22:7d84b8bc20b4 675 H(2,9+j) = -dcm(3,j);
NaotoMorita 22:7d84b8bc20b4 676 //H(3,9+j) = -dcm(3,j);
NaotoMorita 22:7d84b8bc20b4 677 }
NaotoMorita 22:7d84b8bc20b4 678
NaotoMorita 22:7d84b8bc20b4 679 Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+Rvsc);
NaotoMorita 22:7d84b8bc20b4 680 Matrix z(2,1);
NaotoMorita 22:7d84b8bc20b4 681 z << nomVb(1,1) << nomVb(3,1) ;
NaotoMorita 22:7d84b8bc20b4 682 Matrix corrVal = K * (z-H*errState);
NaotoMorita 22:7d84b8bc20b4 683 errState = errState + corrVal;
NaotoMorita 22:7d84b8bc20b4 684 Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*(Rvsc)*MatrixMath::Transpose(K);
NaotoMorita 22:7d84b8bc20b4 685
NaotoMorita 22:7d84b8bc20b4 686 }
NaotoMorita 22:7d84b8bc20b4 687
NaotoMorita 22:7d84b8bc20b4 688 void ScErrStateEKF::updateConstraints(Vector3 gyro)
NaotoMorita 22:7d84b8bc20b4 689 {
NaotoMorita 22:7d84b8bc20b4 690
NaotoMorita 22:7d84b8bc20b4 691 gyro -=gyroBias;
NaotoMorita 22:7d84b8bc20b4 692
NaotoMorita 22:7d84b8bc20b4 693 Matrix dcm(3,3);
NaotoMorita 22:7d84b8bc20b4 694 computeDcm(dcm, qhat);
NaotoMorita 22:7d84b8bc20b4 695
NaotoMorita 22:7d84b8bc20b4 696 Matrix qeterm = -2.0f*MatrixMath::Transpose(dcm)*MatrixMath::Matrixcross(gyro.x,gyro.y,gyro.z);
NaotoMorita 22:7d84b8bc20b4 697 Matrix H(4,nState);
NaotoMorita 22:7d84b8bc20b4 698 for(int i = 1; i<3; i++){
NaotoMorita 22:7d84b8bc20b4 699 for(int j = 1;j<4;j++){
NaotoMorita 22:7d84b8bc20b4 700 H(i,j) = qeterm(i,j);
NaotoMorita 22:7d84b8bc20b4 701 }
NaotoMorita 22:7d84b8bc20b4 702 }
NaotoMorita 22:7d84b8bc20b4 703 H(1,4) = 1.0f*(dcm(1,1));
NaotoMorita 22:7d84b8bc20b4 704 H(1,5) = 1.0f*(dcm(2,1));
NaotoMorita 22:7d84b8bc20b4 705 H(1,6) = 1.0f*(dcm(3,1));
NaotoMorita 22:7d84b8bc20b4 706 H(2,4) = 1.0f*(dcm(1,2));
NaotoMorita 22:7d84b8bc20b4 707 H(2,5) = 1.0f*(dcm(2,2));
NaotoMorita 22:7d84b8bc20b4 708 H(2,6) = 1.0f*(dcm(3,2));
NaotoMorita 22:7d84b8bc20b4 709
NaotoMorita 22:7d84b8bc20b4 710 Matrix nomVb = dcm*vihat;
NaotoMorita 22:7d84b8bc20b4 711 qeterm = -2.0f*MatrixMath::Matrixcross(nomVb(1,1),nomVb(2,1),nomVb(3,1));
NaotoMorita 22:7d84b8bc20b4 712 for(int j = 1;j<4;j++){
NaotoMorita 22:7d84b8bc20b4 713 H(3,j) = -qeterm(1,j);
NaotoMorita 22:7d84b8bc20b4 714 H(4,j) = -qeterm(3,j);
NaotoMorita 22:7d84b8bc20b4 715 //H(3,j) = -qeterm(3,j);
NaotoMorita 22:7d84b8bc20b4 716 H(3,9+j) = -dcm(1,j);
NaotoMorita 22:7d84b8bc20b4 717 H(4,9+j) = -dcm(3,j);
NaotoMorita 22:7d84b8bc20b4 718 //H(3,9+j) = -dcm(3,j);
NaotoMorita 22:7d84b8bc20b4 719 }
NaotoMorita 22:7d84b8bc20b4 720
NaotoMorita 22:7d84b8bc20b4 721 Matrix R(4,4);
NaotoMorita 22:7d84b8bc20b4 722 R(1,1) = Rgsc(1,1);
NaotoMorita 22:7d84b8bc20b4 723 R(2,2) = Rgsc(2,1);
NaotoMorita 22:7d84b8bc20b4 724 R(3,3) = Rvsc(1,1);
NaotoMorita 22:7d84b8bc20b4 725 R(4,4) = Rvsc(2,1);
NaotoMorita 22:7d84b8bc20b4 726 //R(5,5) = Rvsc(3,1);
NaotoMorita 22:7d84b8bc20b4 727 Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
NaotoMorita 22:7d84b8bc20b4 728 Matrix z(4,1);
NaotoMorita 22:7d84b8bc20b4 729 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 730 Matrix corrVal = K * (z-H*errState);
NaotoMorita 22:7d84b8bc20b4 731 errState = errState + corrVal;
NaotoMorita 22:7d84b8bc20b4 732 Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*(R)*MatrixMath::Transpose(K);
NaotoMorita 22:7d84b8bc20b4 733 }
NaotoMorita 22:7d84b8bc20b4 734
NaotoMorita 22:7d84b8bc20b4 735 void ScErrStateEKF::computeVb(Vector3& vb)
NaotoMorita 22:7d84b8bc20b4 736 {
NaotoMorita 22:7d84b8bc20b4 737 Matrix dcm(3,3);
NaotoMorita 22:7d84b8bc20b4 738 computeDcm(dcm, qhat);
NaotoMorita 22:7d84b8bc20b4 739 Matrix vbmat = dcm*vihat;
NaotoMorita 22:7d84b8bc20b4 740 vb.x = vbmat(1,1);
NaotoMorita 22:7d84b8bc20b4 741 vb.y = vbmat(2,1);
NaotoMorita 22:7d84b8bc20b4 742 vb.z = vbmat(3,1);
NaotoMorita 22:7d84b8bc20b4 743
NaotoMorita 22:7d84b8bc20b4 744 }
NaotoMorita 22:7d84b8bc20b4 745 */
NaotoMorita 22:7d84b8bc20b4 746
NaotoMorita 22:7d84b8bc20b4 747