AHRS.cpp
Go to the documentation of this file.
1 /*
2  * @file AHRS.cpp
3  * @brief Attitude and Heading Reference System implementation
4  * Created on: Jan 26, 2012
5  * Author: cbeall3
6  */
7 
8 #include "AHRS.h"
9 #include <cmath>
10 
11 using namespace std;
12 
13 namespace gtsam {
14 
15 /* ************************************************************************* */
16 Matrix3 AHRS::Cov(const Vector3s& m) {
17  const double num_observations = m.cols();
18  const Vector3 mean = m.rowwise().sum() / num_observations;
19  Vector3s D = m.colwise() - mean;
20  return D * trans(D) / (num_observations - 1);
21 }
22 
23 /* ************************************************************************* */
24 AHRS::AHRS(const Matrix& stationaryU, const Matrix& stationaryF, double g_e,
25  bool flat) :
26  KF_(9) {
27 
28  // Initial state
29  mech0_ = Mechanization_bRn2::initialize(stationaryU, stationaryF, g_e, flat);
30 
31  size_t T = stationaryU.cols();
32 
33  // estimate standard deviation on gyroscope readings
34  Pg_ = Cov(stationaryU);
35  Vector3 sigmas_v_g = (Pg_.diagonal() * T).cwiseSqrt();
36 
37  // estimate standard deviation on accelerometer readings
38  Pa_ = Cov(stationaryF);
39 
40  // Quantities needed for integration
41 
42  // dynamics, Chris' email September 23, 2011 3:38:05 PM EDT
43  double tau_g = 730; // correlation time for gyroscope
44  double tau_a = 730; // correlation time for accelerometer
45 
46  F_g_ = -I_3x3 / tau_g;
47  F_a_ = -I_3x3 / tau_a;
48  Vector3 var_omega_w = 0 * Vector::Ones(3); // TODO
49  Vector3 var_omega_g = (0.0034 * 0.0034) * Vector::Ones(3);
50  Vector3 var_omega_a = (0.034 * 0.034) * Vector::Ones(3);
51  Vector3 sigmas_v_g_sq = sigmas_v_g.array().square();
52  var_w_ << var_omega_w, var_omega_g, sigmas_v_g_sq, var_omega_a;
53 
54  // Quantities needed for aiding
55  sigmas_v_a_ = (T * Pa_.diagonal()).cwiseSqrt();
56 
57  // gravity in nav frame
58  n_g_ = (Vector(3) << 0.0, 0.0, g_e).finished();
59  n_g_cross_ = skewSymmetric(n_g_); // nav frame has Z down !!!
60 }
61 
62 /* ************************************************************************* */
63 std::pair<Mechanization_bRn2, KalmanFilter::State> AHRS::initialize(double g_e) {
64 
65  // Calculate Omega_T, formula 2.80 in Farrell08book
66  double cp = cos(mech0_.bRn().inverse().pitch());
67  double sp = sin(mech0_.bRn().inverse().pitch());
68  double cy = cos(0.0);
69  double sy = sin(0.0);
70  Matrix Omega_T = (Matrix(3, 3) << cy * cp, -sy, 0.0, sy * cp, cy, 0.0, -sp, 0.0, 1.0).finished();
71 
72  // Calculate Jacobian of roll/pitch/yaw wrpt (g1,g2,g3), see doc/ypr.nb
73  Vector b_g = mech0_.b_g(g_e);
74  double g1 = b_g(0);
75  double g2 = b_g(1);
76  double g3 = b_g(2);
77  double g23 = g2 * g2 + g3 * g3;
78  double g123 = g1 * g1 + g23;
79  double f = 1 / (std::sqrt(g23) * g123);
80  Matrix H_g = (Matrix(3, 3) <<
81  0.0, g3 / g23, -(g2 / g23), // roll
82  std::sqrt(g23) / g123, -f * (g1 * g2), -f * (g1 * g3), // pitch
83  0.0, 0.0, 0.0).finished(); // we don't know anything on yaw
84 
85  // Calculate the initial covariance matrix for the error state dx, Farrell08book eq. 10.66
86  Matrix Pa = 0.025 * 0.025 * I_3x3;
87  Matrix P11 = Omega_T * (H_g * (Pa + Pa_) * trans(H_g)) * trans(Omega_T);
88  P11(2, 2) = 0.0001;
89  Matrix P12 = -Omega_T * H_g * Pa;
90 
91  Matrix P_plus_k2 = Matrix(9, 9);
92  P_plus_k2.block<3,3>(0, 0) = P11;
93  P_plus_k2.block<3,3>(0, 3) = Z_3x3;
94  P_plus_k2.block<3,3>(0, 6) = P12;
95 
96  P_plus_k2.block<3,3>(3, 0) = Z_3x3;
97  P_plus_k2.block<3,3>(3, 3) = Pg_;
98  P_plus_k2.block<3,3>(3, 6) = Z_3x3;
99 
100  P_plus_k2.block<3,3>(6, 0) = trans(P12);
101  P_plus_k2.block<3,3>(6, 3) = Z_3x3;
102  P_plus_k2.block<3,3>(6, 6) = Pa;
103 
104  Vector dx = Z_9x1;
105  KalmanFilter::State state = KF_.init(dx, P_plus_k2);
106  return std::make_pair(mech0_, state);
107 }
108 
109 /* ************************************************************************* */
110 std::pair<Mechanization_bRn2, KalmanFilter::State> AHRS::integrate(
111  const Mechanization_bRn2& mech, KalmanFilter::State state,
112  const Vector& u, double dt) {
113 
114  // Integrate full state
115  Mechanization_bRn2 newMech = mech.integrate(u, dt);
116 
117  // Integrate error state Kalman filter
118  // FIXME:
119  //if nargout>1
120  Matrix bRn = mech.bRn().matrix();
121 
122  Matrix9 F_k; F_k.setZero();
123  F_k.block<3,3>(0, 3) = -bRn;
124  F_k.block<3,3>(3, 3) = F_g_;
125  F_k.block<3,3>(6, 6) = F_a_;
126 
127  typedef Eigen::Matrix<double,9,12> Matrix9_12;
128  Matrix9_12 G_k; G_k.setZero();
129  G_k.block<3,3>(0, 0) = -bRn;
130  G_k.block<3,3>(0, 6) = -bRn;
131  G_k.block<3,3>(3, 3) = I_3x3;
132  G_k.block<3,3>(6, 9) = I_3x3;
133 
134  Matrix9 Q_k = G_k * var_w_.asDiagonal() * G_k.transpose();
135  Matrix9 Psi_k = I_9x9 + dt * F_k; // +dt*dt*F_k*F_k/2; // transition matrix
136 
137  // This implements update in section 10.6
138  Matrix9 B; B.setZero();
139  Vector9 u2; u2.setZero();
140  // TODO predictQ should be templated to also take fixed size matrices.
141  KalmanFilter::State newState = KF_.predictQ(state, Psi_k,B,u2,dt*Q_k);
142  return make_pair(newMech, newState);
143 }
144 
145 /* ************************************************************************* */
147  const gtsam::Vector& f, const gtsam::Vector& u, double ge) const {
148 
149  // Subtract the biases
150  Vector f_ = f - mech.x_a();
151  Vector u_ = u - mech.x_g();
152 
153  double mu_f = f_.norm() - ge; // accelerometer same magnitude as local gravity ?
154  double mu_u = u_.norm(); // gyro says we are not maneuvering ?
155  return (std::abs(mu_f)<0.5 && mu_u < 5.0 / 180.0 * M_PI);
156 }
157 
158 /* ************************************************************************* */
159 std::pair<Mechanization_bRn2, KalmanFilter::State> AHRS::aid(
160  const Mechanization_bRn2& mech, KalmanFilter::State state,
161  const Vector& f, bool Farrell) const {
162 
163  Matrix bRn = mech.bRn().matrix();
164 
165  // get gravity in body from accelerometer
166  Vector measured_b_g = mech.x_a() - f;
167 
168  Matrix R, H;
169  Vector z;
170  if (Farrell) {
171  // calculate residual gravity measurement
172  z = n_g_ - trans(bRn) * measured_b_g;
173  H = collect(3, &n_g_cross_, &Z_3x3, &bRn);
174  R = trans(bRn) * ((Vector3) sigmas_v_a_.array().square()).asDiagonal() * bRn;
175  } else {
176  // my measurement prediction (in body frame):
177  // F(:,k) = bias - b_g
178  // F(:,k) = mech.x_a + dx_a - bRn*n_g;
179  // F(:,k) = mech.x_a + dx_a - bRn*(I+P)*n_g;
180  // F(:,k) = mech.x_a + dx_a - b_g - bRn*(rho x n_g); // P = [rho]_x
181  // Hence, the measurement z = b_g - (mech.x_a - F(:,k)) is predicted
182  // from the filter state (dx_a, rho) as dx_a + bRn*(n_g x rho)
183  // z = b_g - (mech.x_a - F(:,k)) = dx_a + bRn*(n_g x rho)
184  z = bRn * n_g_ - measured_b_g;
185  // Now the Jacobian H
186  Matrix b_g = bRn * n_g_cross_;
187  H = collect(3, &b_g, &Z_3x3, &I_3x3);
188  // And the measurement noise, TODO: should be created once where sigmas_v_a is given
189  R = ((Vector3) sigmas_v_a_.array().square()).asDiagonal();
190  }
191 
192 // update the Kalman filter
193  KalmanFilter::State updatedState = KF_.updateQ(state, H, z, R);
194 
195 // update full state (rotation and accelerometer bias)
196  Mechanization_bRn2 newMech = mech.correct(updatedState->mean());
197 
198 // reset KF state
199  Vector dx = Z_9x1;
200  updatedState = KF_.init(dx, updatedState->covariance());
201 
202  return make_pair(newMech, updatedState);
203 }
204 
205 /* ************************************************************************* */
206 std::pair<Mechanization_bRn2, KalmanFilter::State> AHRS::aidGeneral(
207  const Mechanization_bRn2& mech, KalmanFilter::State state,
208  const Vector& f, const Vector& f_previous,
209  const Rot3& R_previous) const {
210 
211  Matrix increment = R_previous.between(mech.bRn()).matrix();
212 
213  // expected - measured
214  Vector z = f - increment * f_previous;
215  //Vector z = increment * f_previous - f;
216  Matrix b_g = skewSymmetric(increment* f_previous);
217  Matrix H = collect(3, &b_g, &I_3x3, &Z_3x3);
218 // Matrix R = diag(emul(sigmas_v_a_, sigmas_v_a_));
219 // Matrix R = diag(Vector3(1.0, 0.2, 1.0)); // good for L_twice
220  Matrix R = Vector3(0.01, 0.0001, 0.01).asDiagonal();
221 
222 // update the Kalman filter
223  KalmanFilter::State updatedState = KF_.updateQ(state, H, z, R);
224 
225 // update full state (rotation and gyro bias)
226  Mechanization_bRn2 newMech = mech.correct(updatedState->mean());
227 
228 // reset KF state
229  Vector dx = Z_9x1;
230  updatedState = KF_.init(dx, updatedState->covariance());
231 
232  return make_pair(newMech, updatedState);
233 }
234 
235 /* ************************************************************************* */
236 void AHRS::print(const std::string& s) const {
237  mech0_.print(s + ".mech0_");
238  gtsam::print((Matrix)F_g_, s + ".F_g");
239  gtsam::print((Matrix)F_a_, s + ".F_a");
240  gtsam::print((Vector)var_w_, s + ".var_w");
241 
242  gtsam::print((Vector)sigmas_v_a_, s + ".sigmas_v_a");
243  gtsam::print((Vector)n_g_, s + ".n_g");
244  gtsam::print((Matrix)n_g_cross_, s + ".n_g_cross");
245 
246  gtsam::print((Matrix)Pg_, s + ".P_g");
247  gtsam::print((Matrix)Pa_, s + ".P_a");
248 
249 }
250 
251 /* ************************************************************************* */
253 }
254 
255 /* ************************************************************************* */
256 
257 } /* namespace gtsam */
void print(const Matrix &A, const string &s, ostream &stream)
Definition: Matrix.cpp:155
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Index cols() const
Matrix3f m
Mechanization_bRn2 mech0_
Initial mechanization state.
Definition: AHRS.h:22
Matrix3 Pa_
Definition: AHRS.h:37
State updateQ(const State &p, const Matrix &H, const Vector &z, const Matrix &Q) const
EIGEN_DEVICE_FUNC Derived & setZero(Index size)
static Mechanization_bRn2 initialize(const Matrix &U, const Matrix &F, const double g_e=0, bool flat=false)
Matrix version of initialize.
double pitch(OptionalJacobian< 1, 3 > H=boost::none) const
Definition: Rot3.cpp:207
Eigen::Vector3d Vector3
Definition: Vector.h:43
Vector3 sigmas_v_a_
measurement sigma
Definition: AHRS.h:33
GaussianDensity::shared_ptr State
Definition: KalmanFilter.h:56
Matrix3 F_g_
gyro bias dynamics
Definition: AHRS.h:26
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:43
#define M_PI
Definition: main.h:78
Rot2 R(Rot2::fromAngle(0.1))
EIGEN_DEVICE_FUNC const SqrtReturnType sqrt() const
std::pair< Mechanization_bRn2, KalmanFilter::State > initialize(double g_e)
Definition: AHRS.cpp:63
Definition: Half.h:150
static char trans
std::pair< Mechanization_bRn2, KalmanFilter::State > integrate(const Mechanization_bRn2 &mech, KalmanFilter::State state, const Vector &u, double dt)
Definition: AHRS.cpp:110
Matrix3 matrix() const
Definition: Rot3M.cpp:219
Mechanization_bRn2 integrate(const Vector3 &u, const double dt) const
Matrix stationaryF
Definition: testAHRS.cpp:20
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy y set format x g set format y g set format x2 g set format y2 g set format z g set angles radians set nogrid set key title set key left top Right noreverse box linetype linewidth samplen spacing width set nolabel set noarrow set nologscale set logscale x set set pointsize set encoding default set nopolar set noparametric set set set set surface set nocontour set clabel set mapping cartesian set nohidden3d set cntrparam order set cntrparam linear set cntrparam levels auto set cntrparam points set size set set xzeroaxis lt lw set x2zeroaxis lt lw set yzeroaxis lt lw set y2zeroaxis lt lw set tics in set ticslevel set tics set mxtics default set mytics default set mx2tics default set my2tics default set xtics border mirror norotate autofreq set ytics border mirror norotate autofreq set ztics border nomirror norotate autofreq set nox2tics set noy2tics set timestamp bottom norotate set rrange[*:*] noreverse nowriteback set trange[*:*] noreverse nowriteback set urange[*:*] noreverse nowriteback set vrange[*:*] noreverse nowriteback set xlabel matrix size set x2label set timefmt d m y n H
Vector u2
Matrix< SCALARB, Dynamic, Dynamic > B
Definition: bench_gemm.cpp:36
Point3 mean(const CONTAINER &points)
mean
Definition: Point3.h:66
Matrix trans(const Matrix &A)
Definition: base/Matrix.h:245
State predictQ(const State &p, const Matrix &F, const Matrix &B, const Vector &u, const Matrix &Q) const
Rot3 inverse() const
inverse of a rotation
Definition: Rot3.h:311
const double dt
double g_e
Definition: testAHRS.cpp:21
std::pair< Mechanization_bRn2, KalmanFilter::State > aidGeneral(const Mechanization_bRn2 &mech, KalmanFilter::State state, const Vector &f, const Vector &f_expected, const Rot3 &R_previous) const
Definition: AHRS.cpp:206
EIGEN_DEVICE_FUNC const CosReturnType cos() const
const Vector3 & x_a() const
const Rot3 & bRn() const
Eigen::VectorXd Vector
Definition: Vector.h:38
std::pair< Mechanization_bRn2, KalmanFilter::State > aid(const Mechanization_bRn2 &mech, KalmanFilter::State state, const Vector &f, bool Farrell=0) const
Definition: AHRS.cpp:159
Matrix stationaryU
Definition: testAHRS.cpp:19
virtual ~AHRS()
Definition: AHRS.cpp:252
bool isAidingAvailable(const Mechanization_bRn2 &mech, const Vector &f, const Vector &u, double ge) const
Definition: AHRS.cpp:146
Eigen::Triplet< double > T
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
RealScalar s
Vector3 n_g_
gravity in nav frame
Definition: AHRS.h:34
KalmanFilter KF_
initial Kalman filter
Definition: AHRS.h:23
State init(const Vector &x0, const SharedDiagonal &P0) const
EIGEN_DEVICE_FUNC const CwiseSqrtReturnType cwiseSqrt() const
Variances var_w_
dynamic noise variances
Definition: AHRS.h:30
traits
Definition: chartTesting.h:28
Matrix3 skewSymmetric(double wx, double wy, double wz)
Definition: base/Matrix.h:404
const Vector3 & x_g() const
Matrix3 Pg_
Definition: AHRS.h:37
const double cy
Matrix3 F_a_
acc bias dynamics
Definition: AHRS.h:27
Pose3 g1(Rot3(), Point3(100.0, 0.0, 300.0))
Class between(const Class &g) const
Definition: Lie.h:51
Mechanization_bRn2 correct(const Vector9 &dx) const
Matrix collect(const std::vector< const Matrix * > &matrices, size_t m, size_t n)
Definition: Matrix.cpp:442
EIGEN_DEVICE_FUNC const SinReturnType sin() const
void print(const std::string &s="") const
print
Matrix3 n_g_cross_
and its skew-symmetric matrix
Definition: AHRS.h:35
Map< Matrix< T, Dynamic, Dynamic, ColMajor >, 0, OuterStride<> > matrix(T *data, int rows, int cols, int stride)
The matrix class, also used for vectors and row-vectors.
Pose3 g2(g1.expmap(h *V1_g1))
#define abs(x)
Definition: datatypes.h:17
void print(const std::string &s="") const
print
Definition: AHRS.cpp:236
Vector3 b_g(double g_e) const
gravity in the body frame
static Matrix3 Cov(const Vector3s &m)
Definition: AHRS.cpp:16


gtsam
Author(s):
autogenerated on Sat May 8 2021 02:41:36