diff --git a/gtsam_unstable/gtsam_unstable.h b/gtsam_unstable/gtsam_unstable.h index fc2fc1fff..27dd8ba1c 100644 --- a/gtsam_unstable/gtsam_unstable.h +++ b/gtsam_unstable/gtsam_unstable.h @@ -28,6 +28,7 @@ class gtsam::KeySet; class gtsam::KeyVector; class gtsam::LevenbergMarquardtParams; class gtsam::ISAM2Params; +class gtsam::GaussianDensity; namespace gtsam { @@ -599,4 +600,29 @@ virtual class InvDepthFactorVariant3b : gtsam::NonlinearFactor { InvDepthFactorVariant3b(size_t poseKey1, size_t poseKey2, size_t landmarkKey, const gtsam::Point2& measured, const gtsam::Cal3_S2* K, const gtsam::noiseModel::Base* model); }; +#include +class Mechanization_bRn2 { + Mechanization_bRn2(); + Mechanization_bRn2(gtsam::Rot3& initial_bRn, Vector initial_x_g, + Vector initial_x_a); + Vector b_g(double g_e); + gtsam::Rot3 bRn(); + Vector x_g(); + Vector x_a(); + static gtsam::Mechanization_bRn2 initialize(Matrix U, Matrix F, double g_e); + gtsam::Mechanization_bRn2 correct(Vector dx) const; + gtsam::Mechanization_bRn2 integrate(Vector u, double dt) const; + void print(string s) const; +}; + +#include +class AHRS { + AHRS(Matrix U, Matrix F, double g_e); + pair initialize(double g_e); + pair integrate(const gtsam::Mechanization_bRn2& mech, gtsam::GaussianDensity* state, Vector u, double dt); + pair aid(const gtsam::Mechanization_bRn2& mech, gtsam::GaussianDensity* state, Vector f, bool Farrel); + pair aidGeneral(const gtsam::Mechanization_bRn2& mech, gtsam::GaussianDensity* state, Vector f, Vector f_expected, const gtsam::Rot3& increment); + void print(string s) const; +}; + } //\namespace gtsam