Loose-GNSS-IMU/Loose-GNSS-IMU/MatrixUtils.cpp
// A function to adjust attitude values in state vector to be between 0 to 2PI
A function to adjust attitude values in state vector to be between 0 to 2PI
A function to adjust attitude values in state vector to be between 0 to 2PI
void NormaliseAttitude(VectorXd& States) {
	States(9)  = normalise(States(9) , -PI / 2., PI / 2.);
	States(10) = normalise(States(10), -PI / 2., PI / 2.);
	States(11) = normalise(States(11), -PI, PI);
}
void NormaliseAttitude ( VectorXd & States ) { States ( 9 ) = normalise ( States ( 9 ) , - PI / 2. , PI / 2. ) ; States ( 10 ) = normalise ( States ( 10 ) , - PI / 2. , PI / 2. ) ; States ( 11 ) = normalise ( States ( 11 ) , - PI , PI ) ; }
A function to adjust attitude values in state vector to be between 0 to 2PI
A function to adjust attitude values in state vector to be between 0 to 2PI