diff --git a/.cproject b/.cproject index 42416eeb8..e5b4e113a 100644 --- a/.cproject +++ b/.cproject @@ -1,5 +1,7 @@ - + + + @@ -307,14 +309,6 @@ true true - - make - -j2 - testGaussianFactor.run - true - true - true - make -j2 @@ -341,6 +335,7 @@ make + tests/testBayesTree.run true false @@ -348,6 +343,7 @@ make + testBinaryBayesNet.run true false @@ -395,6 +391,7 @@ make + testSymbolicBayesNet.run true false @@ -402,6 +399,7 @@ make + tests/testSymbolicFactor.run true false @@ -409,6 +407,7 @@ make + testSymbolicFactorGraph.run true false @@ -424,11 +423,20 @@ make + tests/testBayesTree true false true + + make + -j2 + testGaussianFactor.run + true + true + true + make -j5 @@ -517,22 +525,6 @@ false true - - make - -j2 - all - true - true - true - - - make - -j2 - clean - true - true - true - make -j2 @@ -549,6 +541,22 @@ true true + + make + -j2 + all + true + true + true + + + make + -j2 + clean + true + true + true + make -j2 @@ -573,34 +581,26 @@ true true - + make - -j5 - testValues.run + -j2 + all true true true - + make - -j5 - testOrdering.run + -j2 + check true true true - + make - -j5 - testKey.run - true - true - true - - - make - -j5 - testLinearContainerFactor.run + -j2 + clean true true true @@ -685,26 +685,34 @@ true true - + make - -j2 - all + -j5 + testValues.run true true true - + make - -j2 - check + -j5 + testOrdering.run true true true - + make - -j2 - clean + -j5 + testKey.run + true + true + true + + + make + -j5 + testLinearContainerFactor.run true true true @@ -821,6 +829,14 @@ true true + + make + -j5 + testAHRS.run + true + true + true + make -j5 @@ -1055,6 +1071,7 @@ make + testGraph.run true false @@ -1062,6 +1079,7 @@ make + testJunctionTree.run true false @@ -1069,6 +1087,7 @@ make + testSymbolicBayesNetB.run true false @@ -1236,6 +1255,7 @@ make + testErrors.run true false @@ -1281,14 +1301,6 @@ true true - - make - -j2 - testGaussianFactor.run - true - true - true - make -j2 @@ -1369,6 +1381,14 @@ true true + + make + -j2 + testGaussianFactor.run + true + true + true + make -j2 @@ -1675,7 +1695,6 @@ make - testSimulated2DOriented.run true false @@ -1715,7 +1734,6 @@ make - testSimulated2D.run true false @@ -1723,7 +1741,6 @@ make - testSimulated3D.run true false @@ -1923,7 +1940,6 @@ make - tests/testGaussianISAM2 true false @@ -1945,102 +1961,6 @@ true true - - make - -j2 - testRot3.run - true - true - true - - - make - -j2 - testRot2.run - true - true - true - - - make - -j2 - testPose3.run - true - true - true - - - make - -j2 - timeRot3.run - true - true - true - - - make - -j2 - testPose2.run - true - true - true - - - make - -j2 - testCal3_S2.run - true - true - true - - - make - -j2 - testSimpleCamera.run - true - true - true - - - make - -j2 - testHomography2.run - true - true - true - - - make - -j2 - testCalibratedCamera.run - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - testPoint2.run - true - true - true - make -j3 @@ -2242,6 +2162,7 @@ cpack + -G DEB true false @@ -2249,6 +2170,7 @@ cpack + -G RPM true false @@ -2256,6 +2178,7 @@ cpack + -G TGZ true false @@ -2263,6 +2186,7 @@ cpack + --config CPackSourceConfig.cmake true false @@ -2404,34 +2328,98 @@ true true - + make - -j5 - testSpirit.run + -j2 + testRot3.run true true true - + make - -j5 - testWrap.run + -j2 + testRot2.run true true true - + make - -j5 - check.wrap + -j2 + testPose3.run true true true - + make - -j5 - wrap + -j2 + timeRot3.run + true + true + true + + + make + -j2 + testPose2.run + true + true + true + + + make + -j2 + testCal3_S2.run + true + true + true + + + make + -j2 + testSimpleCamera.run + true + true + true + + + make + -j2 + testHomography2.run + true + true + true + + + make + -j2 + testCalibratedCamera.run + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + testPoint2.run true true true @@ -2475,6 +2463,38 @@ false true + + make + -j5 + testSpirit.run + true + true + true + + + make + -j5 + testWrap.run + true + true + true + + + make + -j5 + check.wrap + true + true + true + + + make + -j5 + wrap + true + true + true + diff --git a/gtsam_unstable/slam/AHRS.cpp b/gtsam_unstable/slam/AHRS.cpp new file mode 100644 index 000000000..bee6bcef6 --- /dev/null +++ b/gtsam_unstable/slam/AHRS.cpp @@ -0,0 +1,244 @@ +/* + * @file AHRS.cpp + * @brief Attitude and Heading Reference System implementation + * Created on: Jan 26, 2012 + * Author: cbeall3 + */ + +#include "AHRS.h" +#include + +using namespace std; + +namespace gtsam { + +Matrix cov(const Matrix& 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); +} + +Matrix I3 = eye(3); +Matrix Z3 = zeros(3, 3); + +/* ************************************************************************* */ +AHRS::AHRS(const Matrix& stationaryU, const Matrix& stationaryF, double g_e) : + KF_(9) { + + // Initial state + mech0_ = Mechanization_bRn2::initialize(stationaryU, stationaryF, g_e); + + size_t T = stationaryU.cols(); + + // estimate standard deviation on gyroscope readings + Pg_ = cov(stationaryU); + Vector sigmas_v_g = esqrt(Pg_.diagonal() * T); + + // estimate standard deviation on accelerometer readings + Pa_ = cov(stationaryF); + + // Quantities needed for integration + + // dynamics, Chris' email September 23, 2011 3:38:05 PM EDT + 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; + Vector var_omega_w = 0 * ones(3); // TODO + Vector var_omega_g = (0.0034 * 0.0034) * ones(3); + Vector var_omega_a = (0.034 * 0.034) * ones(3); + Vector sigmas_v_g_sq = emul(sigmas_v_g, sigmas_v_g); + Vector vars = concatVectors(4, &var_omega_w, &var_omega_g, &sigmas_v_g_sq, + &var_omega_a); + var_w_ = diag(vars); + + // Quantities needed for aiding + sigmas_v_a_ = esqrt(T * Pa_.diagonal()); + + // gravity in nav frame + n_g_ = Vector_(3, 0.0, 0.0, g_e); + n_g_cross_ = skewSymmetric(n_g_); // nav frame has Z down !!! +} + +/* ************************************************************************* */ +std::pair AHRS::initialize(double g_e) { + + // Calculate Omega_T, formula 2.80 in Farrell08book + double cp = cos(mech0_.bRn().inverse().pitch()); + double sp = sin(mech0_.bRn().inverse().pitch()); + double cy = cos(0); + double sy = sin(0); + Matrix Omega_T = Matrix_(3, 3, cy * cp, -sy, 0.0, sy * cp, cy, 0.0, -sp, 0.0, 1.0); + + // Calculate Jacobian of roll/pitch/yaw wrpt (g1,g2,g3), see doc/ypr.nb + Vector b_g = mech0_.b_g(g_e); + double g1 = b_g(0); + double g2 = b_g(1); + double g3 = b_g(2); + double g23 = g2 * g2 + g3 * g3; + double g123 = g1 * g1 + g23; + double f = 1 / (sqrt(g23) * g123); + Matrix H_g = Matrix_(3, 3, + 0.0, g3 / g23, -(g2 / g23), // roll + sqrt(g23) / g123, -f * (g1 * g2), -f * (g1 * g3), // pitch + 0.0, 0.0, 0.0); // we don't know anything on yaw + + // Calculate the initial covariance matrix for the error state dx, Farrell08book eq. 10.66 + Matrix Pa = 0.025 * 0.025 * eye(3); + Matrix P11 = Omega_T * (H_g * (Pa + Pa_) * trans(H_g)) * trans(Omega_T); + P11(2, 2) = 0.0001; + Matrix P12 = -Omega_T * H_g * Pa; + + Matrix P_plus_k2 = Matrix(9, 9); + P_plus_k2.block(0, 0, 3, 3) = P11; + P_plus_k2.block(0, 3, 3, 3) = Z3; + P_plus_k2.block(0, 6, 3, 3) = P12; + + P_plus_k2.block(3, 0, 3, 3) = Z3; + P_plus_k2.block(3, 3, 3, 3) = Pg_; + P_plus_k2.block(3, 6, 3, 3) = Z3; + + P_plus_k2.block(6, 0, 3, 3) = trans(P12); + P_plus_k2.block(6, 3, 3, 3) = Z3; + P_plus_k2.block(6, 6, 3, 3) = Pa; + + Vector dx = zero(9); + KalmanFilter::State state = KF_.init(dx, P_plus_k2); + return std::make_pair(mech0_, state); +} + +/* ************************************************************************* */ +std::pair AHRS::integrate( + const Mechanization_bRn2& mech, KalmanFilter::State state, + const Vector& u, double dt) { + + // Integrate full state + Mechanization_bRn2 newMech = mech.integrate(u, dt); + + // Integrate error state Kalman filter + // FIXME: + //if nargout>1 + Matrix bRn = mech.bRn().matrix(); + Matrix I3 = eye(3); + Matrix Z3 = zeros(3, 3); + + Matrix F_k = zeros(9, 9); + F_k.block(0, 3, 3, 3) = -bRn; + F_k.block(3, 3, 3, 3) = F_g_; + F_k.block(6, 6, 3, 3) = F_a_; + + Matrix G_k = zeros(9, 12); + G_k.block(0, 0, 3, 3) = -bRn; + G_k.block(0, 6, 3, 3) = -bRn; + G_k.block(3, 3, 3, 3) = I3; + G_k.block(6, 9, 3, 3) = I3; + + 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 + + Matrix B = zeros(9, 9); + Vector u2 = zero(9); + KalmanFilter::State newState = KF_.predictQ(state, Psi_k,B,u2,dt*Q_k); + return make_pair(newMech, newState); +} + + +/* ************************************************************************* */ +std::pair AHRS::aid( + const Mechanization_bRn2& mech, KalmanFilter::State state, + const Vector& f, bool Farrell) { + + Matrix bRn = mech.bRn().matrix(); + + // get gravity in body from accelerometer + Vector measured_b_g = mech.x_a() - f; + + Matrix R, H; + Vector z; + if (Farrell) { + // calculate residual gravity measurement + z = n_g_ - trans(bRn) * measured_b_g; + H = collect(3, &n_g_cross_, &Z3, &bRn); + R = trans(bRn) * diag(emul(sigmas_v_a_, sigmas_v_a_)) * bRn; + } else { + // my measurement prediction (in body frame): + // F(:,k) = bias - b_g + // F(:,k) = mech.x_a + dx_a - bRn*n_g; + // F(:,k) = mech.x_a + dx_a - bRn*(I+P)*n_g; + // F(:,k) = mech.x_a + dx_a - b_g - bRn*(rho x n_g); // P = [rho]_x + // b_g - (mech.x_a - F(:,k)) = dx_a + bRn*(n_g x rho); + z = bRn * n_g_ - measured_b_g; + Matrix b_g = bRn * n_g_cross_; + H = collect(3, &b_g, &Z3, &I3); + R = diag(emul(sigmas_v_a_, sigmas_v_a_)); + } + +// update the Kalman filter + KalmanFilter::State updatedState = KF_.updateQ(state, H, z, R); + +// update full state (rotation and accelerometer bias) + Mechanization_bRn2 newMech = mech.correct(updatedState->mean()); + +// reset KF state + Vector dx = zeros(9, 1); + updatedState = KF_.init(dx, updatedState->covariance()); + + return make_pair(newMech, updatedState); +} + +/* ************************************************************************* */ +std::pair AHRS::aidGeneral( + const Mechanization_bRn2& mech, KalmanFilter::State state, + const Vector& f, const Vector& f_previous, + const Rot3& R_previous) { + + Matrix increment = R_previous.between(mech.bRn()).matrix(); + + // expected - measured + 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 R = diag(emul(sigmas_v_a_, sigmas_v_a_)); +// Matrix R = diag(Vector_(3, 1.0, 0.2, 1.0)); // good for L_twice + Matrix R = diag(Vector_(3, 0.01, 0.0001, 0.01)); + +// update the Kalman filter + KalmanFilter::State updatedState = KF_.updateQ(state, H, z, R); + +// update full state (rotation and gyro bias) + Mechanization_bRn2 newMech = mech.correct(updatedState->mean()); + +// reset KF state + Vector dx = zeros(9, 1); + updatedState = KF_.init(dx, updatedState->covariance()); + + return make_pair(newMech, updatedState); +} + +/* ************************************************************************* */ +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(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(Pg_, s + ".P_g"); + gtsam::print(Pa_, s + ".P_a"); + +} + +/* ************************************************************************* */ +AHRS::~AHRS() { +} + +/* ************************************************************************* */ + +} /* namespace gtsam */ diff --git a/gtsam_unstable/slam/AHRS.h b/gtsam_unstable/slam/AHRS.h new file mode 100644 index 000000000..89f8b2b5e --- /dev/null +++ b/gtsam_unstable/slam/AHRS.h @@ -0,0 +1,68 @@ +/* + * AHRS.h + * + * Created on: Jan 26, 2012 + * Author: cbeall3 + */ + +#ifndef AHRS_H_ +#define AHRS_H_ + +#include "Mechanization_bRn2.h" +#include + +namespace gtsam { + +Matrix cov(const Matrix& m); + +class AHRS { + +private: + + // Initial state + Mechanization_bRn2 mech0_; ///< Initial mechanization state + 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 + + // 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 + + Matrix Pg_, Pa_; + +public: + /** + * AHRS constructor + * @param stationaryU initial interval of gyro measurements, 3xn matrix + * @param stationaryF initial interval of accelerator measurements, 3xn matrix + * @param g_e if given, initializes gravity with correct value g_e + */ + AHRS(const Matrix& stationaryU, const Matrix& stationaryF, double g_e); + + std::pair initialize(double g_e); + + std::pair integrate( + const Mechanization_bRn2& mech, KalmanFilter::State state, + const Vector& u, double dt); + + std::pair aid( + const Mechanization_bRn2& mech, KalmanFilter::State state, + const Vector& f, bool Farrell=0); + + std::pair aidGeneral( + const Mechanization_bRn2& mech, KalmanFilter::State state, + const Vector& f, const Vector& f_expected, const Rot3& R_previous); + + /// print + void print(const std::string& s = "") const; + + virtual ~AHRS(); +}; + +} /* namespace gtsam */ +#endif /* AHRS_H_ */ diff --git a/gtsam_unstable/slam/Mechanization_bRn2.cpp b/gtsam_unstable/slam/Mechanization_bRn2.cpp new file mode 100644 index 000000000..ca95843af --- /dev/null +++ b/gtsam_unstable/slam/Mechanization_bRn2.cpp @@ -0,0 +1,101 @@ +/** + * @file Mechanization_bRn.cpp + * @date Jan 25, 2012 + * @author Chris Beall + * @author Frank Dellaert + */ + +#include "Mechanization_bRn2.h" +#include + +namespace gtsam { + +/* ************************************************************************* */ +Mechanization_bRn2 Mechanization_bRn2::initializeVector(const std::list& U, + const std::list& F, const double g_e) { + Matrix Umat = Matrix_(3,U.size(), concatVectors(U)); + Matrix Fmat = Matrix_(3,F.size(), concatVectors(F)); + + return initialize(Umat, Fmat, g_e); +} + + +/* ************************************************************************* */ +Mechanization_bRn2 Mechanization_bRn2::initialize(const Matrix& U, + const Matrix& F, const double g_e) { + + // estimate gyro bias by averaging gyroscope readings (10.62) + Vector x_g = U.rowwise().mean(); + Vector meanF = -F.rowwise().mean(); + + Vector b_g, x_a; + if(g_e == 0) { + // estimate gravity in body frame from accelerometer (10.13) + b_g = meanF; + // estimate accelerometer bias as zero (10.65) + x_a = zero(3); + + } else { + // normalize b_g and attribute remainder to biases + b_g = g_e * meanF/meanF.norm(); + x_a = -(meanF - b_g); + } + + // initialize roll, pitch (eqns. 10.14, 10.15) + double g1=b_g(0); + double g2=b_g(1); + double g3=b_g(2); + + // Farrell08book eqn. 10.14 + double roll = atan2(g2, g3); + // Farrell08book eqn. 10.15 + double pitch = atan2(-g1, sqrt(g2 * g2 + g3 * g3)); + double yaw = 0; + // This returns body-to-nav nRb + Rot3 bRn = Rot3::ypr(yaw, pitch, roll).inverse(); + + return Mechanization_bRn2(bRn, x_g, x_a); +} + +/* ************************************************************************* */ +Mechanization_bRn2 Mechanization_bRn2::correct(const Vector& dx) const { + Vector rho = sub(dx, 0, 3); + + Rot3 delta_nRn = Rot3::rodriguez(rho); + Rot3 bRn = bRn_ * delta_nRn; + + Vector x_g = x_g_ + sub(dx, 3, 6); + Vector x_a = x_a_ + sub(dx, 6, 9); + + return Mechanization_bRn2(bRn, x_g, x_a); +} + +/* ************************************************************************* */ +Mechanization_bRn2 Mechanization_bRn2::integrate(const Vector& u, + const double dt) const { + // integrate rotation nRb based on gyro measurement u and bias x_g + + // get body to inertial (measured in b) from gyro, subtract bias + Vector b_omega_ib = u - x_g_; + + // assume nav to inertial through movement is unknown + Vector b_omega_in = zero(3); + + // get angular velocity on bRn measured in body frame + Vector b_omega_bn = b_omega_in - b_omega_ib; + + // convert to navigation frame + Rot3 nRb = bRn_.inverse(); + Vector n_omega_bn = (nRb*b_omega_bn).vector(); + + // integrate bRn using exponential map, assuming constant over dt + Rot3 delta_nRn = Rot3::rodriguez(n_omega_bn*dt); + Rot3 bRn = bRn_.compose(delta_nRn); + + // implicit updating of biases (variables just do not change) + // x_g = x_g; + // x_a = x_a; + return Mechanization_bRn2(bRn, x_g_, x_a_); +} + +} /* namespace gtsam */ diff --git a/gtsam_unstable/slam/Mechanization_bRn2.h b/gtsam_unstable/slam/Mechanization_bRn2.h new file mode 100644 index 000000000..0a71da06d --- /dev/null +++ b/gtsam_unstable/slam/Mechanization_bRn2.h @@ -0,0 +1,81 @@ +/** + * @file Mechanization_bRn.h + * @date Jan 25, 2012 + * @author Chris Beall + * @author Frank Dellaert + */ + +#pragma once + +#include +#include +#include + +namespace gtsam { + +class Mechanization_bRn2 { + +private: + Rot3 bRn_; ///< rotation from nav to body + Vector x_g_; ///< gyroscope bias + Vector x_a_; ///< accelerometer bias + +public: + + /** + * Constructor + * @param initial_bRn initial rotation from nav to body frame + * @param initial_x_g initial gyro bias + * @param r3 Z-axis of rotated frame + */ + Mechanization_bRn2(const Rot3& initial_bRn = Rot3(), + const Vector& initial_x_g = zero(3), const Vector& initial_x_a = zero(3)) : + bRn_(initial_bRn), x_g_(initial_x_g), x_a_(initial_x_a) { + } + + Vector b_g(double g_e) { + Vector n_g = Vector_(3, 0.0, 0.0, g_e); + return (bRn_ * n_g).vector(); + } + + Rot3 bRn() const {return bRn_; } + Vector x_g() const { return x_g_; } + Vector x_a() const { return x_a_; } + + /** + * Initialize the first Mechanization state + * @param U a list of gyro measurement vectors + * @param F a list of accelerometer measurement vectors + */ + static Mechanization_bRn2 initializeVector(const std::list& U, + const std::list& F, const double g_e = 0); + + static Mechanization_bRn2 initialize(const Matrix& U, + const Matrix& F, const double g_e = 0); + + /** + * Correct AHRS full state given error state dx + * @param obj The current state + * @param dx The error used to correct and return a new state + */ + Mechanization_bRn2 correct(const Vector& dx) const; + + /** + * Integrate to get new state + * @param obj The current state + * @param u gyro measurement to integrate + * @param dt time elapsed since previous state in seconds + */ + Mechanization_bRn2 integrate(const Vector& u, const double dt) const; + + /// print + void print(const std::string& s = "") const { + bRn_.print(s + ".R"); + + gtsam::print(x_g_, s + ".x_g"); + gtsam::print(x_a_, s + ".x_a"); + } + +}; + +} // namespace gtsam diff --git a/gtsam_unstable/slam/doc/ypr.nb b/gtsam_unstable/slam/doc/ypr.nb new file mode 100644 index 000000000..e21cc18f1 --- /dev/null +++ b/gtsam_unstable/slam/doc/ypr.nb @@ -0,0 +1,147 @@ +(* Content-type: application/vnd.wolfram.mathematica *) + +(*** Wolfram Notebook File ***) +(* http://www.wolfram.com/nb *) + +(* CreatedBy='Mathematica 8.0' *) + +(*CacheID: 234*) +(* Internal cache information: +NotebookFileLineBreakTest +NotebookFileLineBreakTest +NotebookDataPosition[ 157, 7] +NotebookDataLength[ 3876, 137] +NotebookOptionsPosition[ 3514, 119] +NotebookOutlinePosition[ 3867, 135] +CellTagsIndexPosition[ 3824, 132] +WindowFrame->Normal*) + +(* Beginning of Notebook Content *) +Notebook[{ +Cell[BoxData[{ + RowBox[{ + RowBox[{"roll", "=", + RowBox[{"ArcTan", "[", + RowBox[{"g3", ",", "g2"}], "]"}]}], ";"}], "\n", + RowBox[{ + RowBox[{"pitch", "=", + RowBox[{"ArcTan", "[", + RowBox[{ + RowBox[{"-", "g1"}], ",", + SqrtBox[ + RowBox[{ + SuperscriptBox["g2", "2"], "+", + SuperscriptBox["g3", "2"]}]]}], "]"}]}], ";"}], "\n"}], "Input", + CellChangeTimes->{{3.535838264320565*^9, 3.535838380686434*^9}, { + 3.53583852660116*^9, 3.5358385374458313`*^9}, {3.5358391717151127`*^9, + 3.535839173489215*^9}}], + +Cell[CellGroupData[{ + +Cell[BoxData[ + RowBox[{ + RowBox[{"(", "\[NoBreak]", GridBox[{ + { + RowBox[{"D", "[", + RowBox[{"roll", ",", "g1"}], "]"}], + RowBox[{"D", "[", + RowBox[{"roll", ",", "g2"}], "]"}], + RowBox[{"D", "[", + RowBox[{"roll", ",", "g3"}], "]"}]}, + { + RowBox[{"D", "[", + RowBox[{"pitch", ",", "g1"}], "]"}], + RowBox[{"D", "[", + RowBox[{"pitch", ",", "g2"}], "]"}], + RowBox[{"D", "[", + RowBox[{"pitch", ",", "g3"}], "]"}]}, + {"0", "0", "0"} + }], "\[NoBreak]", ")"}], "//", "FullSimplify"}]], "Input", + CellChangeTimes->{{3.535838384786139*^9, 3.535838492987266*^9}, { + 3.535838580947555*^9, 3.535838595178028*^9}}], + +Cell[BoxData[ + RowBox[{"{", + RowBox[{ + RowBox[{"{", + RowBox[{"0", ",", + FractionBox["g3", + RowBox[{ + SuperscriptBox["g2", "2"], "+", + SuperscriptBox["g3", "2"]}]], ",", + RowBox[{"-", + FractionBox["g2", + RowBox[{ + SuperscriptBox["g2", "2"], "+", + SuperscriptBox["g3", "2"]}]]}]}], "}"}], ",", + RowBox[{"{", + RowBox[{ + FractionBox[ + SqrtBox[ + RowBox[{ + SuperscriptBox["g2", "2"], "+", + SuperscriptBox["g3", "2"]}]], + RowBox[{ + SuperscriptBox["g1", "2"], "+", + SuperscriptBox["g2", "2"], "+", + SuperscriptBox["g3", "2"]}]], ",", + RowBox[{"-", + FractionBox[ + RowBox[{"g1", " ", "g2"}], + RowBox[{ + SqrtBox[ + RowBox[{ + SuperscriptBox["g2", "2"], "+", + SuperscriptBox["g3", "2"]}]], " ", + RowBox[{"(", + RowBox[{ + SuperscriptBox["g1", "2"], "+", + SuperscriptBox["g2", "2"], "+", + SuperscriptBox["g3", "2"]}], ")"}]}]]}], ",", + RowBox[{"-", + FractionBox[ + RowBox[{"g1", " ", "g3"}], + RowBox[{ + SqrtBox[ + RowBox[{ + SuperscriptBox["g2", "2"], "+", + SuperscriptBox["g3", "2"]}]], " ", + RowBox[{"(", + RowBox[{ + SuperscriptBox["g1", "2"], "+", + SuperscriptBox["g2", "2"], "+", + SuperscriptBox["g3", "2"]}], ")"}]}]]}]}], "}"}], ",", + RowBox[{"{", + RowBox[{"0", ",", "0", ",", "0"}], "}"}]}], "}"}]], "Output", + CellChangeTimes->{{3.5358383879873047`*^9, 3.535838493685678*^9}, + 3.53583853974876*^9, {3.535838581771779*^9, 3.535838595765464*^9}}] +}, Open ]] +}, +WindowSize->{740, 752}, +WindowMargins->{{4, Automatic}, {Automatic, 4}}, +FrontEndVersion->"8.0 for Mac OS X x86 (32-bit, 64-bit Kernel) (October 5, \ +2011)", +StyleDefinitions->"Default.nb" +] +(* End of Notebook Content *) + +(* Internal cache information *) +(*CellTagsOutline +CellTagsIndex->{} +*) +(*CellTagsIndex +CellTagsIndex->{} +*) +(*NotebookFileOutline +Notebook[{ +Cell[557, 20, 544, 16, 75, "Input"], +Cell[CellGroupData[{ +Cell[1126, 40, 692, 20, 61, "Input"], +Cell[1821, 62, 1677, 54, 109, "Output"] +}, Open ]] +} +] +*) + +(* End of internal cache information *) + diff --git a/gtsam_unstable/slam/tests/testAHRS.cpp b/gtsam_unstable/slam/tests/testAHRS.cpp new file mode 100644 index 000000000..f9731113b --- /dev/null +++ b/gtsam_unstable/slam/tests/testAHRS.cpp @@ -0,0 +1,83 @@ +/* + * @file testAHRS.cpp + * @brief Test AHRS + * @author Frank Dellaert + * @author Chris Beall + */ + +#include +#include +#include +#include "../AHRS.h" +#include + +using namespace std; +using namespace gtsam; + +// stationary interval of gyro U and acc F +Matrix stationaryU = trans(Matrix_(3,3,-0.0004,-0.0002,-0.0014,0.0006,-0.0003,0.0007,0.0006,-0.0002,-0.0003)); +Matrix stationaryF = trans(Matrix_(3,3,0.1152,-0.0188,9.7419,-0.0163,0.0146,9.7753,-0.0283,-0.0428,9.9021)); +double g_e = 9.7963; // Atlanta + +/* ************************************************************************* */ +TEST (AHRS, cov) { + + // samples stored by row + Matrix A = Matrix_(4, 3, + 1.0, 2.0, 3.0, + 5.0, 7.0, 0.0, + 9.0, 4.0, 7.0, + 6.0, 3.0, 2.0); + + Matrix actual = cov(trans(A)); + Matrix expected = Matrix_(3, 3, + 10.9167, 2.3333, 5.0000, + 2.3333, 4.6667, -2.6667, + 5.0000, -2.6667, 8.6667); + + EXPECT(assert_equal(expected, actual, 1e-4)); +} + +/* ************************************************************************* */ +TEST (AHRS, covU) { + + Matrix actual = cov(10000*stationaryU); + Matrix expected = Matrix_(3, 3, + 33.3333333, -1.66666667, 53.3333333, + -1.66666667, 0.333333333, -5.16666667, + 53.3333333, -5.16666667, 110.333333); + + EXPECT(assert_equal(expected, actual, 1e-4)); +} + +/* ************************************************************************* */ +TEST (AHRS, covF) { + + Matrix actual = cov(100*stationaryF); + Matrix expected = Matrix_(3, 3, + 63.3808333, -0.432166667, -48.1706667, + -0.432166667, 8.31053333, -16.6792667, + -48.1706667, -16.6792667, 71.4297333); + + EXPECT(assert_equal(expected, actual, 1e-4)); +} + +/* ************************************************************************* */ +TEST (AHRS, constructor) { + AHRS ahrs = AHRS(stationaryU,stationaryF,g_e); +} + +/* ************************************************************************* */ +/* TODO: currently fails because of problem with ill-conditioned system +TEST (AHRS, init) { + AHRS ahrs = AHRS(stationaryU,stationaryF,g_e); + std::pair result = ahrs.initialize(g_e); +} +*/ +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ +