HAPSRG
Eigen
Dependencies: Eigen
Dependents: optWingforHAPS_Eigen hexaTest_Eigen
Diff: solaESKF.cpp
- Revision:
- 78:e36a7b844fb5
- Parent:
- 77:780ce6556041
- Child:
- 79:365ea9277167
--- a/solaESKF.cpp Tue Nov 30 11:26:56 2021 +0000
+++ b/solaESKF.cpp Mon Dec 06 08:24:13 2021 +0000
@@ -1,80 +1,73 @@
#include "solaESKF.hpp"
-
-
solaESKF::solaESKF()
- :pihat(3,1),vihat(3,1),qhat(4,1),accBias(3,1),gyroBias(3,1),gravity(3,1),errState(18,1),Phat(18,18),Q(18,18)
+// :pihat(3,1),vihat(3,1),qhat(4,1),accBias(3,1),gyroBias(3,1),gravity(3,1),errState(18,1),Phat(18,18),Q(18,18)
+ :errState(18,1),Phat(18,18),Q(18,18)
{
- pihat << 0.0f << 0.0f << 0.0f;
- vihat << 0.0f << 0.0f << 0.0f;
- qhat << 1.0f << 0.0f << 0.0f << 0.0f;
- accBias << 0.0f << 0.0f << 0.0f;
- gyroBias << 0.0f << 0.0f << 0.0f;
- gravity << 0.0f << 0.0f << 9.8f;
-
- nState = errState.getRows();
+ pihat << 0.0f, 0.0f, 0.0f;
+ vihat << 0.0f, 0.0f, 0.0f;
+ qhat << 1.0f, 0.0f, 0.0f, 0.0f;
+ accBias << 0.0f, 0.0f, 0.0f;
+ gyroBias << 0.0f, 0.0f, 0.0f;
+ gravity << 0.0f, 0.0, 9.8f;
- for (int i = 1; i < nState+1; i++){
- errState(i,1) = 0.0f;
- for (int j = 1; j < nState+1; j++){
- Phat(i,j) = 0.0f;
- Q(i,j) = 0.0f;
- }
- }
+ nState = 18;
+ errState = VectorXf::Zero(nState);
+ Phat = MatrixXf::Zero(nState, nState);
+ Q = MatrixXf::Zero(nState, nState);
- setBlockDiag(Phat,0.1f,1,3);//position
- setBlockDiag(Phat,0.1f,4,6);//velocity
- setBlockDiag(Phat,0.1f,7,9);//angle error
- setBlockDiag(Phat,0.1f,10,12);//acc bias
- setBlockDiag(Phat,0.1f,13,15);//gyro bias
- setBlockDiag(Phat,0.00000001f,16,18);//gravity
- setBlockDiag(Q,0.00025f,4,6);//velocity
- setBlockDiag(Q,0.005f/57.0f,7,9);//angle error
- setBlockDiag(Q,0.001f,10,12);//acc bias
- setBlockDiag(Q,0.001f,13,15);//gyro bias//positionとgravityはQ項なし
-
+ setBlockDiag(Phat, 0.1f, 0, 2);//position
+ setBlockDiag(Phat, 0.1f, 3, 5);//velocity
+ setBlockDiag(Phat, 0.1f, 6, 8);//angle error
+ setBlockDiag(Phat, 0.1f, 9, 11);//acc bias
+ setBlockDiag(Phat, 0.1f, 12, 14);//gyro bias
+ setBlockDiag(Phat, 0.00000001f, 15, 17);//gravity
+ setBlockDiag(Q, 0.00025f, 3, 5);//velocity
+ setBlockDiag(Q, 0.005f/57.0f, 6, 8);//angle error
+ setBlockDiag(Q, 0.001f, 9, 11);//acc bias
+ setBlockDiag(Q, 0.001f, 12, 14);//gyro bias//positionとgravityはQ項なし
}
-void solaESKF::updateNominal(Matrix acc, Matrix gyro,float att_dt)
+void solaESKF::updateNominal(Vector3f acc, Vector3f gyro,float att_dt)
{
- Matrix gyrom = gyro - gyroBias;
- Matrix accm = acc - accBias;
+ Vector3f gyrom = gyro - gyroBias;
+ Vector3f accm = acc - accBias;
- Matrix qint(4,1);
- qint << 1.0f << 0.5f*gyrom(1,1)*att_dt << 0.5f*gyrom(2,1)*att_dt << 0.5f*gyrom(3,1)*att_dt;
- qhat = quatmultiply(qhat,qint);
- float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qhat),qhat));
- qhat *= (1.0f/ qnorm);
+ Vector4f qint;
+ qint << 1.0f, 0.5f*gyrom(0)*att_dt, 0.5f*gyrom(1)*att_dt, 0.5f*gyrom(2)*att_dt;
+ qhat = quatmultiply(qhat, qint);
+ qhat.normalize();
- Matrix dcm(3,3);
+ Matrix3f dcm;
computeDcm(dcm, qhat);
- Matrix accned = dcm*accm+gravity;
+ Vector3f accned = dcm*accm + gravity;
vihat += accned*att_dt;
- pihat += vihat*att_dt+0.5f*accned*att_dt*att_dt;
+ pihat += vihat*att_dt + 0.5f*accned*att_dt*att_dt;
}
-void solaESKF::updateErrState(Matrix acc,Matrix gyro,float att_dt)
+void solaESKF::updateErrState(Vector3f acc, Vector3f gyro, float att_dt)
{
- Matrix gyrom = gyro - gyroBias;
- Matrix accm = acc - accBias;
+ Vector3f gyrom = gyro - gyroBias;
+ Vector3f accm = acc - accBias;
- Matrix dcm(3,3);
+ Matrix3f dcm;
computeDcm(dcm, qhat);
- Matrix a2v = -dcm*MatrixMath::Matrixcross(accm(1,1),accm(2,1),accm(3,1))*att_dt;
- Matrix a2v2 = 0.5f*a2v*att_dt;
+// Matrix a2v = -dcm*MatrixMath::Matrixcross(accm(1,1),accm(2,1),accm(3,1))*att_dt;
+ Matrix3f a2v = -dcm*solaESKF::Matrixcross(accm)*att_dt;
+ Matrix3f a2v2 = 0.5f*a2v*att_dt;
- Matrix Fx(nState,nState);
+ MatrixXf Fx(nState,nState);
//position
+ Fx(0,0) = 1.0f;
Fx(1,1) = 1.0f;
Fx(2,2) = 1.0f;
- Fx(3,3) = 1.0f;
+ Fx(0,3) = 1.0f*att_dt;
Fx(1,4) = 1.0f*att_dt;
Fx(2,5) = 1.0f*att_dt;
- Fx(3,6) = 1.0f*att_dt;
- for (int i = 1; i < 4; i++){
- for (int j = 1; j < 4; j++){
+ for (int i = 0; i < 3; i++){
+ for (int j = 0; j < 3; j++){
Fx(i,j+6) = a2v2(i,j);
Fx(i,j+9) = -0.5f*dcm(i,j)*att_dt*att_dt;
}
@@ -83,97 +76,98 @@
//velocity
+ Fx(3,3) = 1.0f;
Fx(4,4) = 1.0f;
Fx(5,5) = 1.0f;
- Fx(6,6) = 1.0f;
- for (int i = 1; i < 4; i++){
- for (int j = 1; j < 4; j++){
+ for (int i = 0; i < 3; i++){
+ for (int j = 0; j < 3; j++){
Fx(i+3,j+6) = a2v(i,j);
Fx(i+3,j+9) = -dcm(i,j)*att_dt;
Fx(i+3,j+12) = -a2v2(i,j);
}
}
+ Fx(3,15) = 1.0f*att_dt;
Fx(4,16) = 1.0f*att_dt;
Fx(5,17) = 1.0f*att_dt;
- Fx(6,18) = 1.0f*att_dt;
//angulat error
+ Fx(6,6) = 1.0f;
Fx(7,7) = 1.0f;
Fx(8,8) = 1.0f;
- Fx(9,9) = 1.0f;
- Fx(7,8) = gyrom(3,1)*att_dt;
- Fx(7,9) = -gyrom(2,1)*att_dt;
- Fx(8,7) = -gyrom(3,1)*att_dt;
- Fx(8,9) = gyrom(1,1)*att_dt;
- Fx(9,7) = gyrom(2,1)*att_dt;
- Fx(9,8) = -gyrom(1,1)*att_dt;
+ Fx(6,7) = gyrom(2)*att_dt;
+ Fx(6,8) = -gyrom(1)*att_dt;
+ Fx(7,6) = -gyrom(2)*att_dt;
+ Fx(7,8) = gyrom(0)*att_dt;
+ Fx(8,6) = gyrom(1)*att_dt;
+ Fx(8,7) = -gyrom(0)*att_dt;
+ Fx(6,12) = -1.0f*att_dt;
Fx(7,13) = -1.0f*att_dt;
Fx(8,14) = -1.0f*att_dt;
- Fx(9,15) = -1.0f*att_dt;
//acc bias
+ Fx(9,9) = 1.0f;
Fx(10,10) = 1.0f;
Fx(11,11) = 1.0f;
- Fx(12,12) = 1.0f;
//gyro bias
+ Fx(12,12) = 1.0f;
Fx(13,13) = 1.0f;
Fx(14,14) = 1.0f;
- Fx(15,15) = 1.0f;
//gravity bias
+ Fx(15,15) = 1.0f;
Fx(16,16) = 1.0f;
Fx(17,17) = 1.0f;
- Fx(18,18) = 1.0f;
errState = Fx * errState;
- Phat = Fx*Phat*MatrixMath::Transpose(Fx);
- for (int i = 1; i < nState+1; i++){
- if(i>3 && i<10){
+ Phat = Fx*Phat*Fx.transpose();
+ for (int i = 0; i < nState; i++){
+ if(i>2 && i<9){
Phat(i,i) += Q(i,i)*att_dt;
- }else if(i>9 && i<16){
+ }else if(i>8 && i<15){
Phat(i,i) += Q(i,i)* att_dt*att_dt;
}
}
}
-void solaESKF::updateAcc(Matrix acc,Matrix R)
+void solaESKF::updateAcc(Vector3f acc, Matrix3f R)
{
- Matrix accm = acc - accBias;
- Matrix dcm(3,3);
+ Vector3f accm = acc - accBias;
+ Matrix3f dcm;
computeDcm(dcm, qhat);
- Matrix tdcm = MatrixMath::Transpose(dcm);
- Matrix tdcm_g = tdcm*gravity;
- Matrix rotgrav = MatrixMath::Matrixcross(tdcm_g(1,1),tdcm_g(2,1),tdcm_g(3,1));
+ Matrix3f tdcm = dcm.transpose();
+ Vector3f tdcm_g = tdcm*gravity;
+ Matrix3f rotgrav = solaESKF::Matrixcross(tdcm_g);
- Matrix H(3,nState);
- for (int i = 1; i < 4; i++){
- for (int j = 1; j < 4; j++){
+ MatrixXf H(3,nState);
+ for (int i = 0; i < 3; i++){
+ for (int j = 0; j < 3; j++){
H(i,j+6) = rotgrav(i,j);
H(i,j+15) = tdcm(i,j);
}
}
+ H(0,9) = -1.0f;
H(1,10) = -1.0f;
H(2,11) = -1.0f;
- H(3,12) = -1.0f;
- Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
- Matrix zacc = -accm-tdcm*gravity;
- Matrix z(3,1);
- z << zacc(1,1) << zacc(2,1) << zacc(3,1);
+// Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
+ MatrixXf K = (Phat*H.transpose())*(H*Phat*H.transpose()+R).inverse();
+ Vector3f zacc = -accm-tdcm*gravity;
+ Vector3f z;
+ z = zacc;
errState = K * z;
- Phat = (MatrixMath::Eye(nState)-K*H)*Phat;
+ Phat = (MatrixXf::Identity(nState, nState)-K*H)*Phat;
fuseErr2Nominal();
}
-void solaESKF::updateHeading(float a,Matrix R)
+void solaESKF::updateHeading(float a, Matrix3f R)
{
- float q0 = qhat(1,1);
- float q1 = qhat(2,1);
- float q2 = qhat(3,1);
- float q3 = qhat(4,1);
+ float q0 = qhat(0);
+ float q1 = qhat(1);
+ float q2 = qhat(2);
+ float q3 = qhat(3);
bool canUseA = false;
const float SA0 = 2*q3;
@@ -184,7 +178,7 @@
if ((SA3*SA3) > 1e-6f) {
SA4 = 1.0F/(SA3*SA3);
SA5_inv = SA2*SA2*SA4 + 1;
- canUseA = fabsf(SA5_inv) > 1e-6f;
+ canUseA = std::abs(SA5_inv) > 1e-6f;
}
bool canUseB = false;
@@ -196,119 +190,119 @@
if ((SB2*SB2) > 1e-6f) {
SB3 = 1.0F/(SB2*SB2);
SB5_inv = SB3*SB4*SB4 + 1;
- canUseB = fabsf(SB5_inv) > 1e-6f;
+ canUseB = std::abs(SB5_inv) > 1e-6f;
}
- Matrix Hh(1,4);
+ Matrix<float, 1, 4> Hh;
- if (canUseA && (!canUseB || fabsf(SA5_inv) >= fabsf(SB5_inv))) {
+ if (canUseA && (!canUseB || std::abs(SA5_inv) >= std::abs(SB5_inv))) {
const float SA5 = 1.0F/SA5_inv;
const float SA6 = 1.0F/SA3;
const float SA7 = SA2*SA4;
const float SA8 = 2*SA7;
const float SA9 = 2*SA6;
- Hh(1,1) = SA5*(SA0*SA6 - SA8*q0);
- Hh(1,2) = SA5*(SA1*SA6 - SA8*q1);
- Hh(1,3) = SA5*(SA1*SA7 + SA9*q1);
- Hh(1,4) = SA5*(SA0*SA7 + SA9*q0);
- } else if (canUseB && (!canUseA || fabsf(SB5_inv) > fabsf(SA5_inv))) {
+ Hh(0,0) = SA5*(SA0*SA6 - SA8*q0);
+ Hh(0,1) = SA5*(SA1*SA6 - SA8*q1);
+ Hh(0,2) = SA5*(SA1*SA7 + SA9*q1);
+ Hh(0,3) = SA5*(SA0*SA7 + SA9*q0);
+ } else if (canUseB && (!canUseA || std::abs(SB5_inv) > std::abs(SA5_inv))) {
const float SB5 = 1.0F/SB5_inv;
const float SB6 = 1.0F/SB2;
const float SB7 = SB3*SB4;
const float SB8 = 2*SB7;
const float SB9 = 2*SB6;
- Hh(1,1) = -SB5*(SB0*SB6 - SB8*q3);
- Hh(1,2) = -SB5*(SB1*SB6 - SB8*q2);
- Hh(1,3) = -SB5*(-SB1*SB7 - SB9*q2);
- Hh(1,4) = -SB5*(-SB0*SB7 - SB9*q3);
+ Hh(0,0) = -SB5*(SB0*SB6 - SB8*q3);
+ Hh(0,1) = -SB5*(SB1*SB6 - SB8*q2);
+ Hh(0,2) = -SB5*(-SB1*SB7 - SB9*q2);
+ Hh(0,3) = -SB5*(-SB0*SB7 - SB9*q3);
} else {
return;
}
- Matrix Hdq(4,3);
- Hdq << -0.5f*q1 << -0.5f*q2 << -0.5f*q3
- << 0.5f*q0 << -0.5f*q3 << 0.5f*q2
- << 0.5f*q3 << 0.5f*q0 << -0.5f*q1
- << -0.5f*q2 << 0.5f*q1 << 0.5f*q0;
+ Matrix<float, 4, 3> Hdq;
+ Hdq << -0.5f*q1, -0.5f*q2, -0.5f*q3,
+ 0.5f*q0, -0.5f*q3, 0.5f*q2,
+ 0.5f*q3, 0.5f*q0, -0.5f*q1,
+ -0.5f*q2, 0.5f*q1, 0.5f*q0;
- Matrix Hpart = Hh*Hdq;
- Matrix H(1,nState);
- for(int j=1;j<4;j++){
- H(1,j+6) = Hpart(1,j);
+ Matrix<float, 1, 3> Hpart = Hh*Hdq;
+ MatrixXf H(1,nState);
+ for(int j=0; j<3; j++){
+ H(0,j+6) = Hpart(0,j);
}
- const float psi = atan2f(qhat(2,1)*qhat(3,1) + qhat(1,1)*qhat(4,1), 0.5f - qhat(3,1)*qhat(3,1) - qhat(4,1)*qhat(4,1));
- Matrix z(1,1);
- z << atan2f(sin(a-psi),cos(a-psi));
- Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
+ const float psi = std::atan2(qhat(1)*qhat(2) + qhat(0)*qhat(3), 0.5f - qhat(2)*qhat(2) - qhat(3)*qhat(3));
+ Matrix<float, 1, 1> z;
+ z << std::atan2(std::sin(a-psi), std::cos(a-psi));
+ MatrixXf K = (Phat*H.transpose())*(H*Phat*H.transpose()+R).inverse();
errState = K * z;
- Phat = (MatrixMath::Eye(nState)-K*H)*Phat;
+ Phat = (MatrixXf::Identity(nState, nState)-K*H)*Phat;
fuseErr2Nominal();
}
-void solaESKF::updateGPSPosition(Matrix posgps,float palt,Matrix R)
+void solaESKF::updateGPSPosition(Vector3f posgps,float palt,Matrix3f R)
{
- Matrix H(3,nState);
+ MatrixXf H(3,nState);
+ H(0,0) = 1.0f;
H(1,1) = 1.0f;
H(2,2) = 1.0f;
- H(3,3) = 1.0f;
//Matrix A = H*Phat*MatrixMath::Transpose(H)+R;
//Matrix K = (Phat*MatrixMath::Transpose(H))*(MatrixMath::Inv(MatrixMath::Transpose(A)*A+1000.0f*MatrixMath::Eye(3))*MatrixMath::Transpose(A));
- Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
- Matrix z(3,1);
- z << posgps(1,1) - pihat(1,1) << posgps(2,1)-pihat(2,1) << palt - pihat(3,1);
+ MatrixXf K = (Phat*H.transpose())*(H*Phat*H.transpose()+R).inverse();
+ Vector3f z;
+ z << posgps(0)-pihat(0), posgps(1)-pihat(1), palt - pihat(2);
errState = K * z;
- Phat = (MatrixMath::Eye(nState)-K*H)*Phat;
+ Phat = (MatrixXf::Identity(nState, nState)-K*H)*Phat;
//Phat = Phat-K*(H*Phat*MatrixMath::Transpose(H)+R)*MatrixMath::Transpose(K);
//Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*R*MatrixMath::Transpose(K);
fuseErr2Nominal();
}
-void solaESKF::updateGPSVelocity(Matrix velgps,Matrix R)
+void solaESKF::updateGPSVelocity(Vector3f velgps,Matrix3f R)
{
- Matrix H(3,nState);
+ MatrixXf H(3,nState);
+ H(0,3) = 1.0f;
H(1,4) = 1.0f;
H(2,5) = 1.0f;
- H(3,6) = 1.0f;
//Matrix A = H*Phat*MatrixMath::Transpose(H)+R;
//Matrix K = (Phat*MatrixMath::Transpose(H))*(MatrixMath::Inv(MatrixMath::Transpose(A)*A+1000.0f*MatrixMath::Eye(3))*MatrixMath::Transpose(A));
- Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
- Matrix z(3,1);
- z << velgps(1,1) - vihat(1,1) << velgps(2,1)-vihat(2,1)<< velgps(3,1)-vihat(3,1);
+ MatrixXf K = (Phat*H.transpose())*(H*Phat*H.transpose()+R).inverse();
+ Vector3f z;
+ z << velgps(0)-vihat(0), velgps(1)-vihat(1), velgps(2)-vihat(2);
errState = K * z;
- Phat = (MatrixMath::Eye(nState)-K*H)*Phat;
+ Phat = (MatrixXf::Identity(nState, nState)-K*H)*Phat;
//Phat = Phat-K*(H*Phat*MatrixMath::Transpose(H)+R)*MatrixMath::Transpose(K);
//Phat = (MatrixMath::Eye(nState)-K*H)*Phat*MatrixMath::Transpose(MatrixMath::Eye(nState)-K*H)+K*R*MatrixMath::Transpose(K);
fuseErr2Nominal();
}
-void solaESKF::updateGPS(Matrix posgps,float palt,Matrix velgps,Matrix R)
+void solaESKF::updateGPS(Vector3f posgps, float palt, Vector3f velgps, MatrixXf R)
{
- Matrix H(5,nState);
+ MatrixXf H(5, nState);
+ H(0,0) = 1.0f;
H(1,1) = 1.0f;
H(2,2) = 1.0f;
H(3,3) = 1.0f;
H(4,4) = 1.0f;
- H(5,5) = 1.0f;
- Matrix K = (Phat*MatrixMath::Transpose(H))*MatrixMath::Inv(H*Phat*MatrixMath::Transpose(H)+R);
- Matrix z(5,1);
- z << posgps(1,1) - pihat(1,1) << posgps(2,1)-pihat(2,1) << palt - pihat(3,1) << velgps(1,1) - vihat(1,1) << velgps(2,1)-vihat(2,1);
+ MatrixXf K = (Phat*H.transpose())*(H*Phat*H.transpose()+R).inverse();
+ Matrix<float, 5, 1> z;
+ z << posgps(0)-pihat(0), posgps(1)-pihat(1), palt-pihat(2), velgps(0)-vihat(0), velgps(1)-vihat(1);
errState = K * z;
- Phat = (MatrixMath::Eye(nState)-K*H)*Phat;
+ Phat = (MatrixXf::Identity(nState, nState)-K*H)*Phat;
fuseErr2Nominal();
}
-Matrix solaESKF::computeAngles()
+Vector3f solaESKF::computeAngles()
{
- Matrix euler(3,1);
- euler(1,1) = atan2f(qhat(1,1)*qhat(2,1) + qhat(3,1)*qhat(4,1), 0.5f - qhat(2,1)*qhat(2,1) - qhat(3,1)*qhat(3,1));
- euler(2,1) = asinf(-2.0f * (qhat(2,1)*qhat(4,1) - qhat(1,1)*qhat(3,1)));
- euler(3,1) = atan2f(qhat(2,1)*qhat(3,1) + qhat(1,1)*qhat(4,1), 0.5f - qhat(3,1)*qhat(3,1) - qhat(4,1)*qhat(4,1));
+ Vector3f euler;
+ euler(0) = std::atan2(qhat(0)*qhat(1) + qhat(2)*qhat(3), 0.5f - qhat(1)*qhat(1) - qhat(2)*qhat(2));
+ euler(1) = std::asin(-2.0f * (qhat(1)*qhat(3) - qhat(0)*qhat(2)));
+ euler(2) = std::atan2(qhat(1)*qhat(2) + qhat(0)*qhat(3), 0.5f - qhat(2)*qhat(2) - qhat(3)*qhat(3));
return euler;
}
@@ -316,190 +310,190 @@
void solaESKF::fuseErr2Nominal()
{
//position
- pihat(1,1) += errState(1,1);
- pihat(2,1) += errState(2,1);
- pihat(3,1) += errState(3,1);
+ pihat(0) += errState(0);
+ pihat(1) += errState(1);
+ pihat(2) += errState(2);
//velocity
- vihat(1,1) += errState(4,1);
- vihat(2,1) += errState(5,1);
- vihat(3,1) += errState(6,1);
+ vihat(0) += errState(3);
+ vihat(1) += errState(4);
+ vihat(2) += errState(5);
//angle error
- Matrix qerr(4,1);
- qerr << 1.0f << 0.5f*errState(7,1) << 0.5f*errState(8,1) << 0.5f*errState(9,1);
+ Vector4f qerr;
+ qerr << 1.0f, 0.5f*errState(6), 0.5f*errState(7), 0.5f*errState(8);
qhat = quatmultiply(qhat, qerr);
- float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qhat),qhat));
- qhat *= (1.0f/ qnorm);
+ qhat.normalize();
//acc bias
- accBias(1,1) += errState(10,1);
- accBias(2,1) += errState(11,1);
- accBias(3,1) += errState(12,1);
+ accBias(0) += errState(9);
+ accBias(1) += errState(10);
+ accBias(2) += errState(11);
//gyro bias
- gyroBias(1,1) += errState(13,1);
- gyroBias(2,1) += errState(14,1);
- gyroBias(3,1) += errState(15,1);
+ gyroBias(0) += errState(12);
+ gyroBias(1) += errState(13);
+ gyroBias(2) += errState(14);
//gravity bias
- gravity(1,1) += errState(16,1);
- gravity(2,1) += errState(17,1);
- gravity(3,1) += errState(18,1);
-
+ gravity(0) += errState(15);
+ gravity(1) += errState(16);
+ gravity(2) += errState(17);
-
- for (int i = 1; i < nState+1; i++){
- errState(i,1) = 0.0f;
- }
-
+ errState = VectorXf::Zero(nState);
}
-Matrix solaESKF::quatmultiply(Matrix p, Matrix q)
+Vector4f solaESKF::quatmultiply(Vector4f p, Vector4f q)
{
-
- Matrix qout(4,1);
- qout(1,1) = p(1,1)*q(1,1) - p(2,1)*q(2,1) - p(3,1)*q(3,1) - p(4,1)*q(4,1);
- qout(2,1) = p(1,1)*q(2,1) + p(2,1)*q(1,1) + p(3,1)*q(4,1) - p(4,1)*q(3,1);
- qout(3,1) = p(1,1)*q(3,1) - p(2,1)*q(4,1) + p(3,1)*q(1,1) + p(4,1)*q(2,1);
- qout(4,1) = p(1,1)*q(4,1) + p(2,1)*q(3,1) - p(3,1)*q(2,1) + p(4,1)*q(1,1);
- float qnorm = sqrt(MatrixMath::dot(MatrixMath::Transpose(qout),qout));
- qout *= (1.0f/ qnorm);
+ Vector4f qout;
+ qout(0) = p(0)*q(0) - p(1)*q(1) - p(2)*q(2) - p(3)*q(3);
+ qout(1) = p(0)*q(1) + p(1)*q(0) + p(2)*q(3) - p(3)*q(2);
+ qout(2) = p(0)*q(2) - p(1)*q(3) + p(2)*q(0) + p(3)*q(1);
+ qout(3) = p(0)*q(3) + p(1)*q(2) - p(2)*q(1) + p(3)*q(0);
+// qout.normalize();
return qout;
}
-void solaESKF::computeDcm(Matrix& dcm, Matrix quat)
+void solaESKF::computeDcm(Matrix3f& dcm, Vector4f quat)
{
-
- dcm(1,1) = quat(1,1)*quat(1,1) + quat(2,1)*quat(2,1) - quat(3,1)*quat(3,1) - quat(4,1)*quat(4,1);
- dcm(1,2) = 2.0f*(quat(2,1)*quat(3,1) - quat(1,1)*quat(4,1));
- dcm(1,3) = 2.0f*(quat(2,1)*quat(4,1) + quat(1,1)*quat(3,1));
- dcm(2,1) = 2.0f*(quat(2,1)*quat(3,1) + quat(1,1)*quat(4,1));
- dcm(2,2) = quat(1,1)*quat(1,1) - quat(2,1)*quat(2,1) + quat(3,1)*quat(3,1) - quat(4,1)*quat(4,1);
- dcm(2,3) = 2.0f*(quat(3,1)*quat(4,1) - quat(1,1)*quat(2,1));
- dcm(3,1) = 2.0f*(quat(2,1)*quat(4,1) - quat(1,1)*quat(3,1));
- dcm(3,2) = 2.0f*(quat(3,1)*quat(4,1) + quat(1,1)*quat(2,1));
- dcm(3,3) = quat(1,1)*quat(1,1) - quat(2,1)*quat(2,1) - quat(3,1)*quat(3,1) + quat(4,1)*quat(4,1);
-
+// dcm(1,1) = quat(1,1)*quat(1,1) + quat(2,1)*quat(2,1) - quat(3,1)*quat(3,1) - quat(4,1)*quat(4,1);
+// dcm(1,2) = 2.0f*(quat(2,1)*quat(3,1) - quat(1,1)*quat(4,1));
+// dcm(1,3) = 2.0f*(quat(2,1)*quat(4,1) + quat(1,1)*quat(3,1));
+// dcm(2,1) = 2.0f*(quat(2,1)*quat(3,1) + quat(1,1)*quat(4,1));
+// dcm(2,2) = quat(1,1)*quat(1,1) - quat(2,1)*quat(2,1) + quat(3,1)*quat(3,1) - quat(4,1)*quat(4,1);
+// dcm(2,3) = 2.0f*(quat(3,1)*quat(4,1) - quat(1,1)*quat(2,1));
+// dcm(3,1) = 2.0f*(quat(2,1)*quat(4,1) - quat(1,1)*quat(3,1));
+// dcm(3,2) = 2.0f*(quat(3,1)*quat(4,1) + quat(1,1)*quat(2,1));
+// dcm(3,3) = quat(1,1)*quat(1,1) - quat(2,1)*quat(2,1) - quat(3,1)*quat(3,1) + quat(4,1)*quat(4,1);
+
+ dcm(0,0) = quat(0)*quat(0) + quat(1)*quat(1) - quat(2)*quat(2) - quat(3)*quat(3);
+ dcm(0,1) = 2.0f*(quat(1)*quat(2) - quat(0)*quat(3));
+ dcm(0,2) = 2.0f*(quat(1)*quat(3) + quat(0)*quat(2));
+ dcm(1,0) = 2.0f*(quat(1)*quat(2) + quat(0)*quat(3));
+ dcm(1,1) = quat(0)*quat(0) - quat(1)*quat(1) + quat(2)*quat(2) - quat(3)*quat(3);
+ dcm(1,2) = 2.0f*(quat(2)*quat(3) - quat(0)*quat(1));
+ dcm(2,0) = 2.0f*(quat(1)*quat(3) - quat(0)*quat(2));
+ dcm(2,1) = 2.0f*(quat(2)*quat(3) + quat(0)*quat(1));
+ dcm(2,2) = quat(0)*quat(0) - quat(1)*quat(1) - quat(2)*quat(2) + quat(3)*quat(3);
}
void solaESKF::setQhat(float ex,float ey,float ez)
{
- float cos_z_2 = cosf(0.5f*ez);
- float cos_y_2 = cosf(0.5f*ey);
- float cos_x_2 = cosf(0.5f*ex);
+ float cos_z_2 = std::cos(0.5f*ez);
+ float cos_y_2 = std::cos(0.5f*ey);
+ float cos_x_2 = std::cos(0.5f*ex);
- float sin_z_2 = sinf(0.5f*ez);
- float sin_y_2 = sinf(0.5f*ey);
- float sin_x_2 = sinf(0.5f*ex);
+ float sin_z_2 = std::sin(0.5f*ez);
+ float sin_y_2 = std::sin(0.5f*ey);
+ float sin_x_2 = std::sin(0.5f*ex);
// and now compute quaternion
- qhat(1,1) = cos_z_2*cos_y_2*cos_x_2 + sin_z_2*sin_y_2*sin_x_2;
- qhat(2,1) = cos_z_2*cos_y_2*sin_x_2 - sin_z_2*sin_y_2*cos_x_2;
- qhat(3,1) = cos_z_2*sin_y_2*cos_x_2 + sin_z_2*cos_y_2*sin_x_2;
- qhat(4,1) = sin_z_2*cos_y_2*cos_x_2 - cos_z_2*sin_y_2*sin_x_2;
+ qhat(0) = cos_z_2*cos_y_2*cos_x_2 + sin_z_2*sin_y_2*sin_x_2;
+ qhat(1) = cos_z_2*cos_y_2*sin_x_2 - sin_z_2*sin_y_2*cos_x_2;
+ qhat(2) = cos_z_2*sin_y_2*cos_x_2 + sin_z_2*cos_y_2*sin_x_2;
+ qhat(3) = sin_z_2*cos_y_2*cos_x_2 - cos_z_2*sin_y_2*sin_x_2;
}
-Matrix solaESKF::calcDynAcc(Matrix acc)
+Vector3f solaESKF::calcDynAcc(Vector3f acc)
{
- Matrix accm = acc - accBias;
- Matrix tdcm(3,3);
+ Vector3f accm = acc - accBias;
+ Matrix3f tdcm(3,3);
computeDcm(tdcm, qhat);
- tdcm = MatrixMath::Transpose(tdcm);
+ tdcm = tdcm.transpose();
- Matrix dynAcc = accm+tdcm*gravity;
+ Vector3f dynAcc = accm+tdcm*gravity;
return dynAcc;
}
void solaESKF::setGravity(float gx,float gy,float gz)
{
- gravity(1,1) = gx;
- gravity(2,1) = gy;
- gravity(3,1) = gz;
+ gravity(0) = gx;
+ gravity(1) = gy;
+ gravity(2) = gz;
}
-Matrix solaESKF::getPihat()
+Vector3f solaESKF::getPihat()
{
return pihat;
}
-Matrix solaESKF::getVihat()
+Vector3f solaESKF::getVihat()
{
return vihat;
}
-Matrix solaESKF::getQhat()
+Vector4f solaESKF::getQhat()
{
return qhat;
}
-Matrix solaESKF::getAccBias()
+Vector3f solaESKF::getAccBias()
{
return accBias;
}
-Matrix solaESKF::getGyroBias()
+Vector3f solaESKF::getGyroBias()
{
return gyroBias;
}
-Matrix solaESKF::getGravity()
+Vector3f solaESKF::getGravity()
{
return gravity;
}
-Matrix solaESKF::getErrState()
+VectorXf solaESKF::getErrState()
{
return errState;
}
void solaESKF::setPhatPosition(float val)
{
- setBlockDiag(Phat,val,1,3);
+ setBlockDiag(Phat, val, 0,2);
}
void solaESKF::setPhatVelocity(float val)
{
- setBlockDiag(Phat,val,4,6);
+ setBlockDiag(Phat, val, 3,5);
}
void solaESKF::setPhatAngleError(float val)
{
- setBlockDiag(Phat,val,7,9);
+ setBlockDiag(Phat, val, 6,8);
}
void solaESKF::setPhatAccBias(float val)
{
- setBlockDiag(Phat,val,10,12);
+ setBlockDiag(Phat, val, 9,11);
}
void solaESKF::setPhatGyroBias(float val)
{
- setBlockDiag(Phat,val,13,15);
+ setBlockDiag(Phat, val, 12,14);
}
void solaESKF::setPhatGravity(float val)
{
- setBlockDiag(Phat,val,16,18);
+ setBlockDiag(Phat, val, 15,17);
}
void solaESKF::setQVelocity(float val)
{
- setBlockDiag(Q,val,4,6);
+ setBlockDiag(Q, val, 3, 5);
}
void solaESKF::setQAngleError(float val)
{
- setBlockDiag(Q,val,7,9);
+ setBlockDiag(Q, val, 6, 8);
}
void solaESKF::setQAccBias(float val)
{
- setBlockDiag(Q,val,10,12);
+ setBlockDiag(Q, val, 9, 11);
}
void solaESKF::setQGyroBias(float val)
{
- setBlockDiag(Q,val,13,15);
+ setBlockDiag(Q, val, 12, 14);
}
-void solaESKF::setDiag(Matrix& mat, float val){
- for (int i = 1; i < mat.getCols()+1; i++){
+void solaESKF::setDiag(MatrixXf& mat, float val){
+ for (int i = 0; i < mat.cols(); i++){
mat(i,i) = val;
}
}
-void solaESKF::setBlockDiag(Matrix& mat, float val,int startIndex, int endIndex){
+void solaESKF::setBlockDiag(MatrixXf& mat, float val, int startIndex, int endIndex){
for (int i = startIndex; i < endIndex+1; i++){
mat(i,i) = val;
}
@@ -507,11 +501,19 @@
void solaESKF::setPihat(float pi_x, float pi_y)
{
- pihat(1,1) = pi_x;
- pihat(2,1) = pi_y;
+ pihat(0) = pi_x;
+ pihat(1) = pi_y;
//pihat(3,1) = pi_z;
}
+Matrix3f solaESKF::Matrixcross(Vector3f v)
+{
+ Matrix3f m;
+ m << 0.0f, -v(2), v(1),
+ v(2), 0.0f, -v(0),
+ -v(1), v(0), 0.0f;
+ return m;
+}
/*
void solaESKF::updateAccConstraints(Matrix acc,float palt,Matrix R)
{