12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Const correctness, comments, and templated Eigen blocks (esp. important in integrate)

release/4.3a0
dellaert 2014-03-06 21:05:11 -05:00
parent a1433dbd31
commit 691e9884d7
2 changed files with 33 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

45
gtsam_unstable/slam/AHRS.cpp
View File

@ -94,17 +94,17 @@ std::pair<Mechanization_bRn2, KalmanFilter::State> AHRS::initialize(double g_e)
Matrix P12 = -Omega_T * H_g * Pa; Matrix P12 = -Omega_T * H_g * Pa;
Matrix P_plus_k2 = Matrix(9, 9); Matrix P_plus_k2 = Matrix(9, 9);
P_plus_k2.block(0, 0, 3, 3) = P11; P_plus_k2.block<3,3>(0, 0) = P11;
P_plus_k2.block(0, 3, 3, 3) = Z3; P_plus_k2.block<3,3>(0, 3) = Z3;
P_plus_k2.block(0, 6, 3, 3) = P12; P_plus_k2.block<3,3>(0, 6) = P12;
P_plus_k2.block(3, 0, 3, 3) = Z3; P_plus_k2.block<3,3>(3, 0) = Z3;
P_plus_k2.block(3, 3, 3, 3) = Pg_; P_plus_k2.block<3,3>(3, 3) = Pg_;
P_plus_k2.block(3, 6, 3, 3) = Z3; P_plus_k2.block<3,3>(3, 6) = Z3;
P_plus_k2.block(6, 0, 3, 3) = trans(P12); P_plus_k2.block<3,3>(6, 0) = trans(P12);
P_plus_k2.block(6, 3, 3, 3) = Z3; P_plus_k2.block<3,3>(6, 3) = Z3;
P_plus_k2.block(6, 6, 3, 3) = Pa; P_plus_k2.block<3,3>(6, 6) = Pa;
Vector dx = zero(9); Vector dx = zero(9);
KalmanFilter::State state = KF_.init(dx, P_plus_k2); KalmanFilter::State state = KF_.init(dx, P_plus_k2);
@ -127,19 +127,20 @@ std::pair<Mechanization_bRn2, KalmanFilter::State> AHRS::integrate(
Matrix Z3 = zeros(3, 3); Matrix Z3 = zeros(3, 3);
Matrix F_k = zeros(9, 9); Matrix F_k = zeros(9, 9);
F_k.block(0, 3, 3, 3) = -bRn; F_k.block<3,3>(0, 3) = -bRn;
F_k.block(3, 3, 3, 3) = F_g_; F_k.block<3,3>(3, 3) = F_g_;
F_k.block(6, 6, 3, 3) = F_a_; F_k.block<3,3>(6, 6) = F_a_;
Matrix G_k = zeros(9, 12); Matrix G_k = zeros(9, 12);
G_k.block(0, 0, 3, 3) = -bRn; G_k.block<3,3>(0, 0) = -bRn;
G_k.block(0, 6, 3, 3) = -bRn; G_k.block<3,3>(0, 6) = -bRn;
G_k.block(3, 3, 3, 3) = I3; G_k.block<3,3>(3, 3) = I3;
G_k.block(6, 9, 3, 3) = I3; G_k.block<3,3>(6, 9) = I3;
Matrix Q_k = G_k * var_w_ * trans(G_k); 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 Psi_k = eye(9) + dt * F_k; // +dt*dt*F_k*F_k/2; // transition matrix
// This implements update in section 10.6
Matrix B = zeros(9, 9); Matrix B = zeros(9, 9);
Vector u2 = zero(9); Vector u2 = zero(9);
KalmanFilter::State newState = KF_.predictQ(state, Psi_k,B,u2,dt*Q_k); KalmanFilter::State newState = KF_.predictQ(state, Psi_k,B,u2,dt*Q_k);
@ -148,21 +149,21 @@ std::pair<Mechanization_bRn2, KalmanFilter::State> AHRS::integrate(
/* ************************************************************************* */ /* ************************************************************************* */
bool AHRS::isAidingAvailable(const Mechanization_bRn2& mech, bool AHRS::isAidingAvailable(const Mechanization_bRn2& mech,
const gtsam::Vector& f, const gtsam::Vector& u, double ge) { const gtsam::Vector& f, const gtsam::Vector& u, double ge) const {
// Subtract the biases // Subtract the biases
Vector f_ = f - mech.x_a(); Vector f_ = f - mech.x_a();
Vector u_ = u - mech.x_g(); Vector u_ = u - mech.x_g();
double mu_f = f_.norm() - ge; double mu_f = f_.norm() - ge; // accelerometer same magnitude as local gravity ?
double mu_u = u_.norm(); double mu_u = u_.norm(); // gyro says we are not maneuvering ?
return (fabs(mu_f)<0.5 && mu_u < 5.0 / 180.0 * 3.1415926); return (fabs(mu_f)<0.5 && mu_u < 5.0 / 180.0 * M_PI);
} }
/* ************************************************************************* */ /* ************************************************************************* */
std::pair<Mechanization_bRn2, KalmanFilter::State> AHRS::aid( std::pair<Mechanization_bRn2, KalmanFilter::State> AHRS::aid(
const Mechanization_bRn2& mech, KalmanFilter::State state, const Mechanization_bRn2& mech, KalmanFilter::State state,
const Vector& f, bool Farrell) { const Vector& f, bool Farrell) const {
Matrix bRn = mech.bRn().matrix(); Matrix bRn = mech.bRn().matrix();
@ -210,7 +211,7 @@ std::pair<Mechanization_bRn2, KalmanFilter::State> AHRS::aid(
std::pair<Mechanization_bRn2, KalmanFilter::State> AHRS::aidGeneral( std::pair<Mechanization_bRn2, KalmanFilter::State> AHRS::aidGeneral(
const Mechanization_bRn2& mech, KalmanFilter::State state, const Mechanization_bRn2& mech, KalmanFilter::State state,
const Vector& f, const Vector& f_previous, const Vector& f, const Vector& f_previous,
const Rot3& R_previous) { const Rot3& R_previous) const {
Matrix increment = R_previous.between(mech.bRn()).matrix(); Matrix increment = R_previous.between(mech.bRn()).matrix();

13
gtsam_unstable/slam/AHRS.h
View File

@ -52,15 +52,22 @@ public:
const Vector& u, double dt); const Vector& u, double dt);
bool isAidingAvailable(const Mechanization_bRn2& mech, bool isAidingAvailable(const Mechanization_bRn2& mech,
const Vector& f, const Vector& u, double ge); const Vector& f, const Vector& u, double ge) const;
/**
* Aid the AHRS with an accelerometer reading, typically called when isAidingAvailable == true
* @param mech current mechanization state
* @param state current Kalman filter state
* @param f accelerometer reading
* @param Farrell
*/
std::pair<Mechanization_bRn2, KalmanFilter::State> aid( std::pair<Mechanization_bRn2, KalmanFilter::State> aid(
const Mechanization_bRn2& mech, KalmanFilter::State state, const Mechanization_bRn2& mech, KalmanFilter::State state,
const Vector& f, bool Farrell=0); const Vector& f, bool Farrell=0) const;
std::pair<Mechanization_bRn2, KalmanFilter::State> aidGeneral( std::pair<Mechanization_bRn2, KalmanFilter::State> aidGeneral(
const Mechanization_bRn2& mech, KalmanFilter::State state, const Mechanization_bRn2& mech, KalmanFilter::State state,
const Vector& f, const Vector& f_expected, const Rot3& R_previous); const Vector& f, const Vector& f_expected, const Rot3& R_previous) const;
/// print /// print
void print(const std::string& s = "") const; void print(const std::string& s = "") const;