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

Fixes to compile on Windows

release/4.3a0
Richard Roberts 2013-05-21 17:24:49 +00:00
parent cd470df672
commit 16c11c70fb
3 changed files with 6 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
gtsam_unstable/slam/AHRS.cpp
View File

@ -80,10 +80,10 @@ std::pair<Mechanization_bRn2, KalmanFilter::State> AHRS::initialize(double g_e)
double g3 = b_g(2);
double g23 = g2 * g2 + g3 * g3;
double g123 = g1 * g1 + g23;
double f = 1 / (sqrt(g23) * g123);
double f = 1 / (std::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
std::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

3
gtsam_unstable/slam/AHRS.h
View File

@ -9,13 +9,14 @@
#define AHRS_H_
#include "Mechanization_bRn2.h"
#include <gtsam_unstable/base/dllexport.h>
#include <gtsam/linear/KalmanFilter.h>
namespace gtsam {
Matrix cov(const Matrix& m);
class AHRS {
class GTSAM_UNSTABLE_EXPORT AHRS {
private:

3
gtsam_unstable/slam/Mechanization_bRn2.h
View File

@ -9,11 +9,12 @@
#include <gtsam/geometry/Rot3.h>
#include <gtsam/base/Vector.h>
#include <gtsam_unstable/base/dllexport.h>
#include <list>
namespace gtsam {
class Mechanization_bRn2 {
class GTSAM_UNSTABLE_EXPORT Mechanization_bRn2 {
private:
Rot3 bRn_; ///< rotation from nav to body