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
11using namespace std;
12
13namespace gtsam {
14
15/* ************************************************************************* */
16Matrix3 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(A: D) / (num_observations - 1);
21}
22
23/* ************************************************************************* */
24AHRS::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(U: stationaryU, F: stationaryF, g_e, flat);
30
31 size_t T = stationaryU.cols();
32
33 // estimate standard deviation on gyroscope readings
34 Pg_ = Cov(m: stationaryU);
35 Vector3 sigmas_v_g = (Pg_.diagonal() * T).cwiseSqrt();
36
37 // estimate standard deviation on accelerometer readings
38 Pa_ = Cov(m: 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(newSize: 3); // TODO
49 Vector3 var_omega_g = (0.0034 * 0.0034) * Vector::Ones(newSize: 3);
50 Vector3 var_omega_a = (0.034 * 0.034) * Vector::Ones(newSize: 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(w: n_g_); // nav frame has Z down !!!
60}
61
62/* ************************************************************************* */
63std::pair<Mechanization_bRn2, KalmanFilter::State> AHRS::initialize(double g_e) {
64
65 // Calculate Omega_T, formula 2.80 in Farrell08book
66 double cp = cos(x: mech0_.bRn().inverse().pitch());
67 double sp = sin(x: mech0_.bRn().inverse().pitch());
68 double cy = cos(x: 0.0);
69 double sy = sin(x: 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(x: g23) * g123);
80 Matrix H_g = (Matrix(3, 3) <<
81 0.0, g3 / g23, -(g2 / g23), // roll
82 std::sqrt(x: 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(A: H_g)) * trans(A: 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>(startRow: 0, startCol: 0) = P11;
93 P_plus_k2.block<3,3>(startRow: 0, startCol: 3) = Z_3x3;
94 P_plus_k2.block<3,3>(startRow: 0, startCol: 6) = P12;
95
96 P_plus_k2.block<3,3>(startRow: 3, startCol: 0) = Z_3x3;
97 P_plus_k2.block<3,3>(startRow: 3, startCol: 3) = Pg_;
98 P_plus_k2.block<3,3>(startRow: 3, startCol: 6) = Z_3x3;
99
100 P_plus_k2.block<3,3>(startRow: 6, startCol: 0) = trans(A: P12);
101 P_plus_k2.block<3,3>(startRow: 6, startCol: 3) = Z_3x3;
102 P_plus_k2.block<3,3>(startRow: 6, startCol: 6) = Pa;
103
104 Vector dx = Z_9x1;
105 KalmanFilter::State state = KF_.init(x0: dx, P0: P_plus_k2);
106 return std::make_pair(x&: mech0_, y&: state);
107}
108
109/* ************************************************************************* */
110std::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>(startRow: 0, startCol: 3) = -bRn;
124 F_k.block<3,3>(startRow: 3, startCol: 3) = F_g_;
125 F_k.block<3,3>(startRow: 6, startCol: 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>(startRow: 0, startCol: 0) = -bRn;
130 G_k.block<3,3>(startRow: 0, startCol: 6) = -bRn;
131 G_k.block<3,3>(startRow: 3, startCol: 3) = I_3x3;
132 G_k.block<3,3>(startRow: 6, startCol: 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(p: state, F: Psi_k,B,u: u2,Q: dt*Q_k);
142 return make_pair(x&: newMech, y&: newState);
143}
144
145/* ************************************************************************* */
146bool AHRS::isAidingAvailable(const Mechanization_bRn2& mech,
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(x: mu_f)<0.5 && mu_u < 5.0 / 180.0 * M_PI);
156}
157
158/* ************************************************************************* */
159std::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(A: bRn) * measured_b_g;
173 H = collect(nrMatrices: 3, &n_g_cross_, &Z_3x3, &bRn);
174 R = trans(A: 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(nrMatrices: 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(p: state, H, z, R);
194
195// update full state (rotation and accelerometer bias)
196 Mechanization_bRn2 newMech = mech.correct(dx: updatedState->mean());
197
198// reset KF state
199 Vector dx = Z_9x1;
200 updatedState = KF_.init(x0: dx, P0: updatedState->covariance());
201
202 return make_pair(x&: newMech, y&: updatedState);
203}
204
205/* ************************************************************************* */
206std::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(g: 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(w: increment* f_previous);
217 Matrix H = collect(nrMatrices: 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(p: state, H, z, R);
224
225// update full state (rotation and gyro bias)
226 Mechanization_bRn2 newMech = mech.correct(dx: updatedState->mean());
227
228// reset KF state
229 Vector dx = Z_9x1;
230 updatedState = KF_.init(x0: dx, P0: updatedState->covariance());
231
232 return make_pair(x&: newMech, y&: updatedState);
233}
234
235/* ************************************************************************* */
236void AHRS::print(const std::string& s) const {
237 mech0_.print(s: s + ".mech0_");
238 gtsam::print(A: (Matrix)F_g_, s: s + ".F_g");
239 gtsam::print(A: (Matrix)F_a_, s: s + ".F_a");
240 gtsam::print(v: (Vector)var_w_, s: s + ".var_w");
241
242 gtsam::print(v: (Vector)sigmas_v_a_, s: s + ".sigmas_v_a");
243 gtsam::print(v: (Vector)n_g_, s: s + ".n_g");
244 gtsam::print(A: (Matrix)n_g_cross_, s: s + ".n_g_cross");
245
246 gtsam::print(A: (Matrix)Pg_, s: s + ".P_g");
247 gtsam::print(A: (Matrix)Pa_, s: s + ".P_a");
248
249}
250
251/* ************************************************************************* */
252AHRS::~AHRS() {
253}
254
255/* ************************************************************************* */
256
257} /* namespace gtsam */
258