"Fixed" AHRS
dellaert
parent
178e4fd61c
commit
d2e53d4fe9
|
|
@ -12,12 +12,12 @@ using namespace std;
|
|||
|
||||
namespace gtsam {
|
||||
|
||||
Matrix cov(const Matrix& m) {
|
||||
/* ************************************************************************* */
|
||||
Matrix3 AHRS::Cov(const Vector3s& m) {
|
||||
const double num_observations = m.cols();
|
||||
const Vector mean = m.rowwise().sum() / num_observations;
|
||||
Matrix D = m.colwise() - mean;
|
||||
Matrix DDt = D * trans(D);
|
||||
return DDt / (num_observations - 1);
|
||||
const Vector3 mean = m.rowwise().sum() / num_observations;
|
||||
Vector3s D = m.colwise() - mean;
|
||||
return D * trans(D) / (num_observations - 1);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -31,11 +31,11 @@ AHRS::AHRS(const Matrix& stationaryU, const Matrix& stationaryF, double g_e,
|
|||
size_t T = stationaryU.cols();
|
||||
|
||||
// estimate standard deviation on gyroscope readings
|
||||
Pg_ = cov(stationaryU);
|
||||
Vector sigmas_v_g = esqrt(Pg_.diagonal() * T);
|
||||
Pg_ = Cov(stationaryU);
|
||||
Vector3 sigmas_v_g = esqrt(Pg_.diagonal() * T);
|
||||
|
||||
// estimate standard deviation on accelerometer readings
|
||||
Pa_ = cov(stationaryF);
|
||||
Pa_ = Cov(stationaryF);
|
||||
|
||||
// Quantities needed for integration
|
||||
|
||||
|
|
@ -43,15 +43,13 @@ AHRS::AHRS(const Matrix& stationaryU, const Matrix& stationaryF, double g_e,
|
|||
double tau_g = 730; // correlation time for gyroscope
|
||||
double tau_a = 730; // correlation time for accelerometer
|
||||
|
||||
F_g_ = -I3 / tau_g;
|
||||
F_a_ = -I3 / tau_a;
|
||||
F_g_ = -I_3x3 / tau_g;
|
||||
F_a_ = -I_3x3 / tau_a;
|
||||
Vector3 var_omega_w = 0 * ones(3); // TODO
|
||||
Vector3 var_omega_g = (0.0034 * 0.0034) * ones(3);
|
||||
Vector3 var_omega_a = (0.034 * 0.034) * ones(3);
|
||||
Vector3 sigmas_v_g_sq = emul(sigmas_v_g, sigmas_v_g);
|
||||
Vector vars(12);
|
||||
vars << var_omega_w, var_omega_g, sigmas_v_g_sq, var_omega_a;
|
||||
var_w_ = diag(vars);
|
||||
Vector3 sigmas_v_g_sq = sigmas_v_g.cwiseProduct(sigmas_v_g);
|
||||
var_w_ << var_omega_w, var_omega_g, sigmas_v_g_sq, var_omega_a;
|
||||
|
||||
// Quantities needed for aiding
|
||||
sigmas_v_a_ = esqrt(T * Pa_.diagonal());
|
||||
|
|
@ -92,15 +90,15 @@ std::pair<Mechanization_bRn2, KalmanFilter::State> AHRS::initialize(double g_e)
|
|||
|
||||
Matrix P_plus_k2 = Matrix(9, 9);
|
||||
P_plus_k2.block<3,3>(0, 0) = P11;
|
||||
P_plus_k2.block<3,3>(0, 3) = Z3;
|
||||
P_plus_k2.block<3,3>(0, 3) = Z_3x3;
|
||||
P_plus_k2.block<3,3>(0, 6) = P12;
|
||||
|
||||
P_plus_k2.block<3,3>(3, 0) = Z3;
|
||||
P_plus_k2.block<3,3>(3, 0) = Z_3x3;
|
||||
P_plus_k2.block<3,3>(3, 3) = Pg_;
|
||||
P_plus_k2.block<3,3>(3, 6) = Z3;
|
||||
P_plus_k2.block<3,3>(3, 6) = Z_3x3;
|
||||
|
||||
P_plus_k2.block<3,3>(6, 0) = trans(P12);
|
||||
P_plus_k2.block<3,3>(6, 3) = Z3;
|
||||
P_plus_k2.block<3,3>(6, 3) = Z_3x3;
|
||||
P_plus_k2.block<3,3>(6, 6) = Pa;
|
||||
|
||||
Vector dx = zero(9);
|
||||
|
|
@ -120,26 +118,26 @@ std::pair<Mechanization_bRn2, KalmanFilter::State> AHRS::integrate(
|
|||
// FIXME:
|
||||
//if nargout>1
|
||||
Matrix bRn = mech.bRn().matrix();
|
||||
Matrix I3 = eye(3);
|
||||
Matrix Z3 = zeros(3, 3);
|
||||
|
||||
Matrix F_k = zeros(9, 9);
|
||||
Matrix9 F_k; F_k.setZero();
|
||||
F_k.block<3,3>(0, 3) = -bRn;
|
||||
F_k.block<3,3>(3, 3) = F_g_;
|
||||
F_k.block<3,3>(6, 6) = F_a_;
|
||||
|
||||
Matrix G_k = zeros(9, 12);
|
||||
typedef Eigen::Matrix<double,9,12> Matrix9_12;
|
||||
Matrix9_12 G_k; G_k.setZero();
|
||||
G_k.block<3,3>(0, 0) = -bRn;
|
||||
G_k.block<3,3>(0, 6) = -bRn;
|
||||
G_k.block<3,3>(3, 3) = I3;
|
||||
G_k.block<3,3>(6, 9) = I3;
|
||||
G_k.block<3,3>(3, 3) = I_3x3;
|
||||
G_k.block<3,3>(6, 9) = I_3x3;
|
||||
|
||||
Matrix Q_k = G_k * var_w_ * trans(G_k);
|
||||
Matrix Psi_k = eye(9) + dt * F_k; // +dt*dt*F_k*F_k/2; // transition matrix
|
||||
Matrix9 Q_k = G_k * var_w_.asDiagonal() * G_k.transpose();
|
||||
Matrix9 Psi_k = I_9x9 + dt * F_k; // +dt*dt*F_k*F_k/2; // transition matrix
|
||||
|
||||
// This implements update in section 10.6
|
||||
Matrix B = zeros(9, 9);
|
||||
Vector u2 = zero(9);
|
||||
Matrix9 B; B.setZero();
|
||||
Vector9 u2; u2.setZero();
|
||||
// TODO predictQ should be templated to also take fixed size matrices.
|
||||
KalmanFilter::State newState = KF_.predictQ(state, Psi_k,B,u2,dt*Q_k);
|
||||
return make_pair(newMech, newState);
|
||||
}
|
||||
|
|
@ -172,7 +170,7 @@ std::pair<Mechanization_bRn2, KalmanFilter::State> AHRS::aid(
|
|||
if (Farrell) {
|
||||
// calculate residual gravity measurement
|
||||
z = n_g_ - trans(bRn) * measured_b_g;
|
||||
H = collect(3, &n_g_cross_, &Z3, &bRn);
|
||||
H = collect(3, &n_g_cross_, &Z_3x3, &bRn);
|
||||
R = trans(bRn) * diag(emul(sigmas_v_a_, sigmas_v_a_)) * bRn;
|
||||
} else {
|
||||
// my measurement prediction (in body frame):
|
||||
|
|
@ -186,7 +184,7 @@ std::pair<Mechanization_bRn2, KalmanFilter::State> AHRS::aid(
|
|||
z = bRn * n_g_ - measured_b_g;
|
||||
// Now the Jacobian H
|
||||
Matrix b_g = bRn * n_g_cross_;
|
||||
H = collect(3, &b_g, &Z3, &I3);
|
||||
H = collect(3, &b_g, &Z_3x3, &I_3x3);
|
||||
// And the measurement noise, TODO: should be created once where sigmas_v_a is given
|
||||
R = diag(emul(sigmas_v_a_, sigmas_v_a_));
|
||||
}
|
||||
|
|
@ -216,7 +214,7 @@ std::pair<Mechanization_bRn2, KalmanFilter::State> AHRS::aidGeneral(
|
|||
Vector z = f - increment * f_previous;
|
||||
//Vector z = increment * f_previous - f;
|
||||
Matrix b_g = skewSymmetric(increment* f_previous);
|
||||
Matrix H = collect(3, &b_g, &I3, &Z3);
|
||||
Matrix H = collect(3, &b_g, &I_3x3, &Z_3x3);
|
||||
// Matrix R = diag(emul(sigmas_v_a_, sigmas_v_a_));
|
||||
// Matrix R = diag(Vector3(1.0, 0.2, 1.0)); // good for L_twice
|
||||
Matrix R = diag(Vector3(0.01, 0.0001, 0.01));
|
||||
|
|
@ -237,16 +235,16 @@ std::pair<Mechanization_bRn2, KalmanFilter::State> AHRS::aidGeneral(
|
|||
/* ************************************************************************* */
|
||||
void AHRS::print(const std::string& s) const {
|
||||
mech0_.print(s + ".mech0_");
|
||||
gtsam::print(F_g_, s + ".F_g");
|
||||
gtsam::print(F_a_, s + ".F_a");
|
||||
gtsam::print(var_w_, s + ".var_w");
|
||||
gtsam::print((Matrix)F_g_, s + ".F_g");
|
||||
gtsam::print((Matrix)F_a_, s + ".F_a");
|
||||
gtsam::print((Vector)var_w_, s + ".var_w");
|
||||
|
||||
gtsam::print(sigmas_v_a_, s + ".sigmas_v_a");
|
||||
gtsam::print(n_g_, s + ".n_g");
|
||||
gtsam::print(n_g_cross_, s + ".n_g_cross");
|
||||
gtsam::print((Vector)sigmas_v_a_, s + ".sigmas_v_a");
|
||||
gtsam::print((Vector)n_g_, s + ".n_g");
|
||||
gtsam::print((Matrix)n_g_cross_, s + ".n_g_cross");
|
||||
|
||||
gtsam::print(Pg_, s + ".P_g");
|
||||
gtsam::print(Pa_, s + ".P_a");
|
||||
gtsam::print((Matrix)Pg_, s + ".P_g");
|
||||
gtsam::print((Matrix)Pa_, s + ".P_a");
|
||||
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -14,8 +14,6 @@
|
|||
|
||||
namespace gtsam {
|
||||
|
||||
GTSAM_UNSTABLE_EXPORT Matrix cov(const Matrix& m);
|
||||
|
||||
class GTSAM_UNSTABLE_EXPORT AHRS {
|
||||
|
||||
private:
|
||||
|
|
@ -25,18 +23,24 @@ private:
|
|||
KalmanFilter KF_; ///< initial Kalman filter
|
||||
|
||||
// Quantities needed for integration
|
||||
Matrix F_g_; ///< gyro bias dynamics
|
||||
Matrix F_a_; ///< acc bias dynamics
|
||||
Matrix var_w_; ///< dynamic noise variances
|
||||
Matrix3 F_g_; ///< gyro bias dynamics
|
||||
Matrix3 F_a_; ///< acc bias dynamics
|
||||
|
||||
typedef Eigen::Matrix<double,12,1> Variances;
|
||||
Variances var_w_; ///< dynamic noise variances
|
||||
|
||||
// Quantities needed for aiding
|
||||
Vector sigmas_v_a_; ///< measurement sigma
|
||||
Vector n_g_; ///< gravity in nav frame
|
||||
Matrix n_g_cross_; ///< and its skew-symmetric matrix
|
||||
Vector3 sigmas_v_a_; ///< measurement sigma
|
||||
Vector3 n_g_; ///< gravity in nav frame
|
||||
Matrix3 n_g_cross_; ///< and its skew-symmetric matrix
|
||||
|
||||
Matrix Pg_, Pa_;
|
||||
Matrix3 Pg_, Pa_;
|
||||
|
||||
public:
|
||||
|
||||
typedef Eigen::Matrix<double,3,Eigen::Dynamic> Vector3s;
|
||||
static Matrix3 Cov(const Vector3s& m);
|
||||
|
||||
/**
|
||||
* AHRS constructor
|
||||
* @param stationaryU initial interval of gyro measurements, 3xn matrix
|
||||
|
|
|
|||
|
|
@ -30,7 +30,7 @@ TEST (AHRS, cov) {
|
|||
9.0, 4.0, 7.0,
|
||||
6.0, 3.0, 2.0).finished();
|
||||
|
||||
Matrix actual = cov(trans(A));
|
||||
Matrix actual = AHRS::Cov(trans(A));
|
||||
Matrix expected = (Matrix(3, 3) <<
|
||||
10.9167, 2.3333, 5.0000,
|
||||
2.3333, 4.6667, -2.6667,
|
||||
|
|
@ -42,7 +42,7 @@ TEST (AHRS, cov) {
|
|||
/* ************************************************************************* */
|
||||
TEST (AHRS, covU) {
|
||||
|
||||
Matrix actual = cov(10000*stationaryU);
|
||||
Matrix actual = AHRS::Cov(10000*stationaryU);
|
||||
Matrix expected = (Matrix(3, 3) <<
|
||||
33.3333333, -1.66666667, 53.3333333,
|
||||
-1.66666667, 0.333333333, -5.16666667,
|
||||
|
|
@ -54,7 +54,7 @@ TEST (AHRS, covU) {
|
|||
/* ************************************************************************* */
|
||||
TEST (AHRS, covF) {
|
||||
|
||||
Matrix actual = cov(100*stationaryF);
|
||||
Matrix actual = AHRS::Cov(100*stationaryF);
|
||||
Matrix expected = (Matrix(3, 3) <<
|
||||
63.3808333, -0.432166667, -48.1706667,
|
||||
-0.432166667, 8.31053333, -16.6792667,
|
||||
|
|
|
|||
Loading…
Reference in New Issue