0.4.1
Loading...
Searching...
No Matches
ProcessNoise.hpp
Go to the documentation of this file.
1// This file is part of INSTINCT, the INS Toolkit for Integrated
2// Navigation Concepts and Training by the Institute of Navigation of
3// the University of Stuttgart, Germany.
4//
5// This Source Code Form is subject to the terms of the Mozilla Public
6// License, v. 2.0. If a copy of the MPL was not distributed with this
7// file, You can obtain one at https://mozilla.org/MPL/2.0/.
8
9/// @file ProcessNoise.hpp
10/// @brief General process Noise definitions
11/// @author T. Topp (topp@ins.uni-stuttgart.de)
12/// @date 2022-01-31
13
14#pragma once
15
16#include <Eigen/Core>
17
22
23namespace NAV
24{
25
26/// @brief Random walk noise input matrix 𝐆
27/// @param[in] sigma2 Variance of the noise on the measurements
28/// @note See T. Hobiger (2021) Inertialnavigation V06 - equation (6.3)
29Eigen::Matrix3d G_RandomWalk(const Eigen::Vector3d& sigma2);
30
31/// @brief Submatrix 𝐆_a of the noise input matrix 𝐆
32/// @param[in] sigma2 Variance of the noise on the measurements
33/// @param[in] beta Gauss-Markov constant 𝛽 = 1 / 𝜏 (𝜏 correlation length)
34/// @note See T. Hobiger (2021) Inertialnavigation V06 - equation (6.3)
35Eigen::Matrix3d G_GaussMarkov1(const Eigen::Vector3d& sigma2, const Eigen::Vector3d& beta);
36
37/// @brief u_bias^2 Power Spectral Density of the bias for a Gauss-Markov random process
38/// @param[in] sigma2_bd 𝜎²_bd standard deviation of the bias noise
39/// @param[in] tau_bd 𝜏 Correlation length in [s]
40/// @note See Brown & Hwang (2011) - Introduction to Random Signals and Applied Kalman Filtering (example 9.6)
41[[nodiscard]] Eigen::Vector3d psd2BiasGaussMarkov(const Eigen::Vector3d& sigma2_bd, const Eigen::Vector3d& tau_bd);
42
43/// @brief Submatrix 𝐐_11 of the system noise covariance matrix 𝐐
44/// @param[in] S_rg Power Spectral Density of the gyroscope random noise
45/// @param[in] S_bgd Power Spectral Density of the gyroscope bias variation
46/// @param[in] tau_s Time interval in [s]
47/// @return The 3x3 matrix 𝐐_11
48/// @note See Groves (2013) equation (14.81)
49[[nodiscard]] Eigen::Matrix3d Q_psi_psi(const Eigen::Vector3d& S_rg, const Eigen::Vector3d& S_bgd, const double& tau_s);
50
51/// @brief Submatrix 𝐐_21 of the system noise covariance matrix 𝐐
52/// @param[in] S_rg Power Spectral Density of the gyroscope random noise
53/// @param[in] S_bgd Power Spectral Density of the gyroscope bias variation
54/// @param[in] ien_F_21 Submatrix 𝐅_21 of the system matrix 𝐅 in {i,e,n} frame
55/// @param[in] tau_s Time interval in [s]
56/// @return The 3x3 matrix 𝐐_21
57/// @note See Groves (2013) equation (14.81)
58[[nodiscard]] Eigen::Matrix3d ien_Q_dv_psi(const Eigen::Vector3d& S_rg, const Eigen::Vector3d& S_bgd, const Eigen::Matrix3d& ien_F_21, const double& tau_s);
59
60/// @brief Submatrix 𝐐_22 of the system noise covariance matrix 𝐐
61/// @param[in] S_ra Power Spectral Density of the accelerometer random noise
62/// @param[in] S_bad Power Spectral Density of the accelerometer bias variation
63/// @param[in] S_rg Power Spectral Density of the gyroscope random noise
64/// @param[in] S_bgd Power Spectral Density of the gyroscope bias variation
65/// @param[in] ien_F_21 Submatrix 𝐅_21 of the system matrix 𝐅 in {i,e,n} frame
66/// @param[in] tau_s Time interval in [s]
67/// @return The 3x3 matrix 𝐐_22
68/// @note See Groves (2013) equation (14.81)
69[[nodiscard]] Eigen::Matrix3d ien_Q_dv_dv(const Eigen::Vector3d& S_ra, const Eigen::Vector3d& S_bad, const Eigen::Vector3d& S_rg, const Eigen::Vector3d& S_bgd, const Eigen::Matrix3d& ien_F_21, const double& tau_s);
70
71/// @brief Submatrix 𝐐_25 of the system noise covariance matrix 𝐐
72/// @param[in] S_bgd Power Spectral Density of the gyroscope bias variation
73/// @param[in] ien_F_21 Submatrix 𝐅_21 of the system matrix 𝐅
74/// @param[in] ien_Dcm_b Direction Cosine Matrix from body to {i,e,n} frame
75/// @param[in] tau_s Time interval in [s]
76/// @return The 3x3 matrix 𝐐_25
77/// @note See Groves (2013) equation (14.80)
78[[nodiscard]] Eigen::Matrix3d ien_Q_dv_domega(const Eigen::Vector3d& S_bgd, const Eigen::Matrix3d& ien_F_21, const Eigen::Matrix3d& ien_Dcm_b, const double& tau_s);
79
80/// @brief Submatrix 𝐐_31 of the system noise covariance matrix 𝐐
81/// @param[in] S_rg Power Spectral Density of the gyroscope random noise
82/// @param[in] S_bgd Power Spectral Density of the gyroscope bias variation
83/// @param[in] n_F_21 Submatrix 𝐅_21 of the system matrix 𝐅
84/// @param[in] T_rn_p Conversion matrix between cartesian and curvilinear perturbations to the position
85/// @param[in] tau_s Time interval in [s]
86/// @return The 3x3 matrix 𝐐_31
87/// @note See Groves (2013) equation (14.81)
88[[nodiscard]] Eigen::Matrix3d n_Q_dr_psi(const Eigen::Vector3d& S_rg, const Eigen::Vector3d& S_bgd, const Eigen::Matrix3d& n_F_21, const Eigen::Matrix3d& T_rn_p, const double& tau_s);
89
90/// @brief Submatrix 𝐐_31 of the system noise covariance matrix 𝐐
91/// @param[in] S_rg Power Spectral Density of the gyroscope random noise
92/// @param[in] S_bgd Power Spectral Density of the gyroscope bias variation
93/// @param[in] ie_F_21 Submatrix 𝐅_21 of the system matrix 𝐅 in {i,e} frame
94/// @param[in] tau_s Time interval in [s]
95/// @return The 3x3 matrix 𝐐_31
96/// @note See Groves (2013) equation (14.81)
97[[nodiscard]] Eigen::Matrix3d ie_Q_dr_psi(const Eigen::Vector3d& S_rg, const Eigen::Vector3d& S_bgd, const Eigen::Matrix3d& ie_F_21, const double& tau_s);
98
99/// @brief Submatrix 𝐐_32 of the system noise covariance matrix 𝐐
100/// @param[in] S_ra Power Spectral Density of the accelerometer random noise
101/// @param[in] S_bad Power Spectral Density of the accelerometer bias variation
102/// @param[in] S_rg Power Spectral Density of the gyroscope random noise
103/// @param[in] S_bgd Power Spectral Density of the gyroscope bias variation
104/// @param[in] n_F_21 Submatrix 𝐅_21 of the system matrix 𝐅
105/// @param[in] T_rn_p Conversion matrix between cartesian and curvilinear perturbations to the position
106/// @param[in] tau_s Time interval in [s]
107/// @return The 3x3 matrix 𝐐_32
108/// @note See Groves (2013) equation (14.81)
109[[nodiscard]] Eigen::Matrix3d n_Q_dr_dv(const Eigen::Vector3d& S_ra, const Eigen::Vector3d& S_bad, const Eigen::Vector3d& S_rg, const Eigen::Vector3d& S_bgd, const Eigen::Matrix3d& n_F_21, const Eigen::Matrix3d& T_rn_p, const double& tau_s);
110
111/// @brief Submatrix 𝐐_32 of the system noise covariance matrix 𝐐
112/// @param[in] S_ra Power Spectral Density of the accelerometer random noise
113/// @param[in] S_bad Power Spectral Density of the accelerometer bias variation
114/// @param[in] S_rg Power Spectral Density of the gyroscope random noise
115/// @param[in] S_bgd Power Spectral Density of the gyroscope bias variation
116/// @param[in] ie_F_21 Submatrix 𝐅_21 of the system matrix 𝐅 in {i,e} frame
117/// @param[in] tau_s Time interval in [s]
118/// @return The 3x3 matrix 𝐐_32
119/// @note See Groves (2013) equation (14.81)
120[[nodiscard]] Eigen::Matrix3d ie_Q_dr_dv(const Eigen::Vector3d& S_ra, const Eigen::Vector3d& S_bad, const Eigen::Vector3d& S_rg, const Eigen::Vector3d& S_bgd, const Eigen::Matrix3d& ie_F_21, const double& tau_s);
121
122/// @brief Submatrix 𝐐_33 of the system noise covariance matrix 𝐐
123/// @param[in] S_ra Power Spectral Density of the accelerometer random noise
124/// @param[in] S_bad Power Spectral Density of the accelerometer bias variation
125/// @param[in] S_rg Power Spectral Density of the gyroscope random noise
126/// @param[in] S_bgd Power Spectral Density of the gyroscope bias variation
127/// @param[in] n_F_21 Submatrix 𝐅_21 of the system matrix 𝐅
128/// @param[in] T_rn_p Conversion matrix between cartesian and curvilinear perturbations to the position
129/// @param[in] tau_s Time interval in [s]
130/// @return The 3x3 matrix 𝐐_33
131/// @note See Groves (2013) equation (14.81)
132[[nodiscard]] Eigen::Matrix3d n_Q_dr_dr(const Eigen::Vector3d& S_ra, const Eigen::Vector3d& S_bad, const Eigen::Vector3d& S_rg, const Eigen::Vector3d& S_bgd, const Eigen::Matrix3d& n_F_21, const Eigen::Matrix3d& T_rn_p, const double& tau_s);
133
134/// @brief Submatrix 𝐐_33 of the system noise covariance matrix 𝐐
135/// @param[in] S_ra Power Spectral Density of the accelerometer random noise
136/// @param[in] S_bad Power Spectral Density of the accelerometer bias variation
137/// @param[in] S_rg Power Spectral Density of the gyroscope random noise
138/// @param[in] S_bgd Power Spectral Density of the gyroscope bias variation
139/// @param[in] ie_F_21 Submatrix 𝐅_21 of the system matrix 𝐅 in {i,e} frame
140/// @param[in] tau_s Time interval in [s]
141/// @return The 3x3 matrix 𝐐_33
142/// @note See Groves (2013) equation (14.81)
143[[nodiscard]] Eigen::Matrix3d ie_Q_dr_dr(const Eigen::Vector3d& S_ra, const Eigen::Vector3d& S_bad, const Eigen::Vector3d& S_rg, const Eigen::Vector3d& S_bgd, const Eigen::Matrix3d& ie_F_21, const double& tau_s);
144
145/// @brief Submatrix 𝐐_34 of the system noise covariance matrix 𝐐
146/// @param[in] S_bad Power Spectral Density of the accelerometer bias variation
147/// @param[in] T_rn_p Conversion matrix between cartesian and curvilinear perturbations to the position
148/// @param[in] n_Dcm_b Direction Cosine Matrix from body to navigation coordinates
149/// @param[in] tau_s Time interval in [s]
150/// @return The 3x3 matrix 𝐐_34
151/// @note See Groves (2013) equation (14.81)
152[[nodiscard]] Eigen::Matrix3d n_Q_dr_df(const Eigen::Vector3d& S_bad, const Eigen::Matrix3d& T_rn_p, const Eigen::Matrix3d& n_Dcm_b, const double& tau_s);
153
154/// @brief Submatrix 𝐐_34 of the system noise covariance matrix 𝐐
155/// @param[in] S_bad Power Spectral Density of the accelerometer bias variation
156/// @param[in] ie_Dcm_b Direction Cosine Matrix from body to {i,e} coordinates
157/// @param[in] tau_s Time interval in [s]
158/// @return The 3x3 matrix 𝐐_34
159/// @note See Groves (2013) equation (14.81)
160[[nodiscard]] Eigen::Matrix3d ie_Q_dr_df(const Eigen::Vector3d& S_bad, const Eigen::Matrix3d& ie_Dcm_b, const double& tau_s);
161
162/// @brief Submatrix 𝐐_35 of the system noise covariance matrix 𝐐
163/// @param[in] S_bgd Power Spectral Density of the gyroscope bias variation
164/// @param[in] n_F_21 Submatrix 𝐅_21 of the system matrix 𝐅
165/// @param[in] T_rn_p Conversion matrix between cartesian and curvilinear perturbations to the position
166/// @param[in] n_Dcm_b Direction Cosine Matrix from body to navigation coordinates
167/// @param[in] tau_s Time interval in [s]
168/// @return The 3x3 matrix 𝐐_35
169/// @note See Groves (2013) equation (14.81)
170[[nodiscard]] Eigen::Matrix3d n_Q_dr_domega(const Eigen::Vector3d& S_bgd, const Eigen::Matrix3d& n_F_21, const Eigen::Matrix3d& T_rn_p, const Eigen::Matrix3d& n_Dcm_b, const double& tau_s);
171
172/// @brief Submatrix 𝐐_35 of the system noise covariance matrix 𝐐
173/// @param[in] S_bgd Power Spectral Density of the gyroscope bias variation
174/// @param[in] ie_F_21 Submatrix 𝐅_21 of the system matrix 𝐅
175/// @param[in] ie_Dcm_b Direction Cosine Matrix from body to {i,e} coordinates
176/// @param[in] tau_s Time interval in [s]
177/// @return The 3x3 matrix 𝐐_35
178/// @note See Groves (2013) equation (14.81)
179[[nodiscard]] Eigen::Matrix3d ie_Q_dr_domega(const Eigen::Vector3d& S_bgd, const Eigen::Matrix3d& ie_F_21, const Eigen::Matrix3d& ie_Dcm_b, const double& tau_s);
180
181/// @brief Submatrix 𝐐_42 of the system noise covariance matrix 𝐐
182/// @param[in] S_bad Power Spectral Density of the accelerometer bias variation
183/// @param[in] b_Dcm_ien Direction Cosine Matrix from {i,e,n} to body coordinates
184/// @param[in] tau_s Time interval in [s]
185/// @return The 3x3 matrix 𝐐_42
186/// @note See Groves (2013) equation (14.80)
187[[nodiscard]] Eigen::Matrix3d Q_df_dv(const Eigen::Vector3d& S_bad, const Eigen::Matrix3d& b_Dcm_ien, const double& tau_s);
188
189/// @brief Submatrix 𝐐_44 of the system noise covariance matrix 𝐐
190/// @param[in] S_bad Power Spectral Density of the accelerometer bias variation
191/// @param[in] tau_s Time interval in [s]
192/// @return The 3x3 matrix 𝐐_44
193/// @note See Groves (2013) equation (14.80)
194[[nodiscard]] Eigen::Matrix3d Q_df_df(const Eigen::Vector3d& S_bad, const double& tau_s);
195
196/// @brief Submatrix 𝐐_51 of the system noise covariance matrix 𝐐
197/// @param[in] S_bgd Power Spectral Density of the gyroscope bias variation
198/// @param[in] b_Dcm_ien Direction Cosine Matrix from {i,e,n} to body coordinates
199/// @param[in] tau_s Time interval in [s]
200/// @return The 3x3 matrix 𝐐_51
201/// @note See Groves (2013) equation (14.80)
202[[nodiscard]] Eigen::Matrix3d Q_domega_psi(const Eigen::Vector3d& S_bgd, const Eigen::Matrix3d& b_Dcm_ien, const double& tau_s);
203
204/// @brief Submatrix 𝐐_55 of the system noise covariance matrix 𝐐
205/// @param[in] S_bgd Power Spectral Density of the gyroscope bias variation
206/// @param[in] tau_s Time interval in [s]
207/// @return The 3x3 matrix 𝐐_55
208/// @note See Groves (2013) equation (14.80)
209[[nodiscard]] Eigen::Matrix3d Q_domega_domega(const Eigen::Vector3d& S_bgd, const double& tau_s);
210
211/// @brief Submatrix 𝐐_66 of the system noise covariance matrix 𝐐
212/// @param[in] S_cPhi Power Spectral Density of the receiver clock phase drift in [m^2 s^-1]
213/// @param[in] S_cf Power Spectral Density of the receiver clock frequency-drift [m^2 s^-3]
214/// @param[in] tau_s Time interval in [s]
215/// @return See Groves (2013) equation (9.152)
216[[nodiscard]] Eigen::Matrix2d Q_gnss(const double& S_cPhi, const double& S_cf, const double& tau_s);
217
218/// @brief Calculate the noise input matrix in Earth frame coordinates
219/// @param b_quat_p Quaternion from IMU platform frame to body frame
220/// @param e_quat_b Quaternion from body to Earth frame
221/// @param accelBiases Flag wether to calculate the rows and columns for the acceleration bias
222/// @param gyroBiases Flag wether to calculate the rows and columns for the angular rate bias
223/// @param accelScaleFactor Flag wether to calculate the rows and columns for the acceleration scale factor
224/// @param gyroScaleFactor Flag wether to calculate the rows and columns for the angular rate scale factor
225/// @param accelMisalignment Flag wether to calculate the rows and columns for the accelerometer misalignment
226/// @param gyroMisalignment Flag wether to calculate the rows and columns for the gyroscope misalignment
227template<typename KeyType, typename T>
228[[nodiscard]] KeyedMatrixX<T, KeyType, KeyType> e_noiseInput_G(const Eigen::Quaterniond& b_quat_p,
229 const Eigen::Quaternion<T>& e_quat_b,
230 bool accelBiases, bool gyroBiases,
231 bool accelScaleFactor, bool gyroScaleFactor,
232 bool accelMisalignment, bool gyroMisalignment)
233
234{
235 std::vector<KeyType> rowKeys;
236 rowKeys.reserve(3 + 3 + 4
237 + 3 * (static_cast<size_t>(accelBiases) + static_cast<size_t>(gyroBiases))
238 + 3 * (static_cast<size_t>(accelScaleFactor) + static_cast<size_t>(gyroScaleFactor))
239 + 4 * (static_cast<size_t>(accelMisalignment) + static_cast<size_t>(gyroMisalignment)));
240 std::vector<KeyType> colKeys;
241 colKeys.reserve(3 + 3 + 3
242 + 3 * (static_cast<size_t>(accelBiases) + static_cast<size_t>(gyroBiases))
243 + 3 * (static_cast<size_t>(accelScaleFactor) + static_cast<size_t>(gyroScaleFactor)));
244
245 constexpr std::array<KeyType, 3> AngleErrorKeys = { Keys::AttQ1, Keys::AttQ2, Keys::AttQ3 }; // 3 quaternion keys are used for the angle error
246
247 rowKeys.insert(rowKeys.end(), Keys::PosVelAtt<KeyType>.begin(), Keys::PosVelAtt<KeyType>.end());
248 colKeys.insert(colKeys.end(), Keys::PosVel<KeyType>.begin(), Keys::PosVel<KeyType>.end());
249 colKeys.insert(colKeys.end(), AngleErrorKeys.begin(), AngleErrorKeys.end());
250
251 if (accelBiases)
252 {
253 rowKeys.insert(rowKeys.end(), Keys::AccelBias<KeyType>.begin(), Keys::AccelBias<KeyType>.end());
254 colKeys.insert(colKeys.end(), Keys::AccelBias<KeyType>.begin(), Keys::AccelBias<KeyType>.end());
255 }
256 if (gyroBiases)
257 {
258 rowKeys.insert(rowKeys.end(), Keys::GyroBias<KeyType>.begin(), Keys::GyroBias<KeyType>.end());
259 colKeys.insert(colKeys.end(), Keys::GyroBias<KeyType>.begin(), Keys::GyroBias<KeyType>.end());
260 }
261 if (accelScaleFactor)
262 {
263 rowKeys.insert(rowKeys.end(), Keys::AccelScaleFactor<KeyType>.begin(), Keys::AccelScaleFactor<KeyType>.end());
264 colKeys.insert(colKeys.end(), Keys::AccelScaleFactor<KeyType>.begin(), Keys::AccelScaleFactor<KeyType>.end());
265 }
266 if (gyroScaleFactor)
267 {
268 rowKeys.insert(rowKeys.end(), Keys::GyroScaleFactor<KeyType>.begin(), Keys::GyroScaleFactor<KeyType>.end());
269 colKeys.insert(colKeys.end(), Keys::GyroScaleFactor<KeyType>.begin(), Keys::GyroScaleFactor<KeyType>.end());
270 }
271 if (accelMisalignment) { rowKeys.insert(rowKeys.end(), Keys::AccelMisalignment<KeyType>.begin(), Keys::AccelMisalignment<KeyType>.end()); }
272 if (gyroMisalignment) { rowKeys.insert(rowKeys.end(), Keys::GyroMisalignment<KeyType>.begin(), Keys::GyroMisalignment<KeyType>.end()); }
273
274 KeyedMatrixX<T, KeyType, KeyType> G(Eigen::MatrixX<T>::Zero(static_cast<int>(rowKeys.size()), static_cast<int>(colKeys.size())), rowKeys, colKeys);
275
276 Eigen::Quaternion<T> e_quat_p = e_quat_b * b_quat_p.cast<T>();
277 G(Keys::Vel<KeyType>, Keys::Vel<KeyType>) = e_quat_p.toRotationMatrix();
278 G(Keys::Att<KeyType>, AngleErrorKeys) = trafo::covRPY2quatJacobian(e_quat_p) * e_quat_p.toRotationMatrix();
279 if (accelBiases) { G(Keys::AccelBias<KeyType>, Keys::AccelBias<KeyType>).setIdentity(); }
280 if (gyroBiases) { G(Keys::GyroBias<KeyType>, Keys::GyroBias<KeyType>).setIdentity(); }
281 if (accelScaleFactor) { G(Keys::AccelScaleFactor<KeyType>, Keys::AccelScaleFactor<KeyType>).setIdentity(); }
282 if (gyroScaleFactor) { G(Keys::GyroScaleFactor<KeyType>, Keys::GyroScaleFactor<KeyType>).setIdentity(); }
283
284 return G;
285}
286
287/// @brief Calculate the noise scale matrix in Earth frame coordinates
288/// @param sigma2_ra 𝜎²_ra Variance of the noise on the accelerometer specific-force measurements [m^2 / s^4 / Hz]
289/// @param sigma2_rg 𝜎²_rg Variance of the noise on the gyro angular-rate measurements [rad^2 / s^2 /Hz]
290/// @param sigma2_bad 𝜎²_bad Variance of the accelerometer dynamic bias [m^2 / s^4]
291/// @param sigma2_bgd 𝜎²_bgd Variance of the gyro dynamic bias [rad^2/s^2]
292/// @param sigma2_sad 𝜎²_sad Variance of the accelerometer dynamic scale factor [-]
293/// @param sigma2_sgd 𝜎²_sgd Variance of the gyro dynamic scale factor [-]
294/// @param accelBiases Flag wether to calculate the rows and columns for the acceleration bias
295/// @param gyroBiases Flag wether to calculate the rows and columns for the angular rate bias
296/// @param accelScaleFactor Flag wether to calculate the rows and columns for the acceleration scale factor
297/// @param gyroScaleFactor Flag wether to calculate the rows and columns for the angular rate scale factor
298template<typename KeyType, typename T>
299[[nodiscard]] KeyedMatrixX<T, KeyType, KeyType> e_noiseScale_W(const Eigen::Vector3d& sigma2_ra, const Eigen::Vector3d& sigma2_rg,
300 const Eigen::Vector3d& sigma2_bad, const Eigen::Vector3d& sigma2_bgd,
301 const Eigen::Vector3d& sigma2_sad, const Eigen::Vector3d& sigma2_sgd,
302 bool accelBiases, bool gyroBiases,
303 bool accelScaleFactor, bool gyroScaleFactor)
304
305{
306 std::vector<KeyType> keys;
307 keys.reserve(3 + 3 + 3
308 + 3 * (static_cast<size_t>(accelBiases) + static_cast<size_t>(gyroBiases))
309 + 3 * (static_cast<size_t>(accelScaleFactor) + static_cast<size_t>(gyroScaleFactor)));
310
311 constexpr std::array<KeyType, 3> AngleErrorKeys = { Keys::AttQ1, Keys::AttQ2, Keys::AttQ3 }; // 3 quaternion keys are used for the angle error
312 keys.insert(keys.end(), Keys::PosVel<KeyType>.begin(), Keys::PosVel<KeyType>.end());
313 keys.insert(keys.end(), AngleErrorKeys.begin(), AngleErrorKeys.end());
314
315 if (accelBiases) { keys.insert(keys.end(), Keys::AccelBias<KeyType>.begin(), Keys::AccelBias<KeyType>.end()); }
316 if (gyroBiases) { keys.insert(keys.end(), Keys::GyroBias<KeyType>.begin(), Keys::GyroBias<KeyType>.end()); }
317 if (accelScaleFactor) { keys.insert(keys.end(), Keys::AccelScaleFactor<KeyType>.begin(), Keys::AccelScaleFactor<KeyType>.end()); }
318 if (gyroScaleFactor) { keys.insert(keys.end(), Keys::GyroScaleFactor<KeyType>.begin(), Keys::GyroScaleFactor<KeyType>.end()); }
319
320 KeyedMatrixX<T, KeyType, KeyType> W(Eigen::MatrixX<T>::Zero(static_cast<int>(keys.size()), static_cast<int>(keys.size())), keys, keys);
321
322 W(Keys::Vel<KeyType>, Keys::Vel<KeyType>).diagonal() = sigma2_ra.cast<T>();
323 W(AngleErrorKeys, AngleErrorKeys).diagonal() = sigma2_rg.cast<T>();
324 if (accelBiases) { W(Keys::AccelBias<KeyType>, Keys::AccelBias<KeyType>).diagonal() = sigma2_bad.cast<T>(); }
325 if (gyroBiases) { W(Keys::GyroBias<KeyType>, Keys::GyroBias<KeyType>).diagonal() = sigma2_bgd.cast<T>(); }
326 if (accelScaleFactor) { W(Keys::AccelScaleFactor<KeyType>, Keys::AccelScaleFactor<KeyType>).diagonal() = sigma2_sad.cast<T>(); }
327 if (gyroScaleFactor) { W(Keys::GyroScaleFactor<KeyType>, Keys::GyroScaleFactor<KeyType>).diagonal() = sigma2_sgd.cast<T>(); }
328
329 return W;
330}
331
332} // namespace NAV
Transformation collection.
Inertial Navigation System Keys.
Motion System Model.
constexpr std::array< StateKeyType, 4 > Att
All attitude keys.
constexpr std::array< StateKeyType, 3 > GyroBias
All gyroscope bias keys.
Definition Keys.hpp:58
constexpr std::array< StateKeyType, 3 > AccelScaleFactor
All accelerometer scale factor keys.
Definition Keys.hpp:62
constexpr std::array< StateKeyType, 4 > AccelMisalignment
All accelerometer misalignment quaternion keys.
Definition Keys.hpp:69
constexpr std::array< StateKeyType, 4 > GyroMisalignment
All gyroscope misalignment quaternion keys.
Definition Keys.hpp:72
@ AttQ3
z: Coefficient of k
@ AttQ1
x: Coefficient of i
@ AttQ2
y: Coefficient of j
constexpr std::array< StateKeyType, 3 > GyroScaleFactor
All gyroscope scale factor keys.
Definition Keys.hpp:65
constexpr std::array< StateKeyType, 10 > PosVelAtt
Vector with all position velocity and attitude keys.
constexpr std::array< StateKeyType, 3 > AccelBias
All accelerometer bias keys.
Definition Keys.hpp:55
constexpr std::array< StateKeyType, 6 > PosVel
Vector with all position and velocity keys.
constexpr std::array< StateKeyType, 3 > Vel
All velocity keys.
Eigen::Matrix< typename Derived::Scalar, 4, 3 > covRPY2quatJacobian(const Eigen::QuaternionBase< Derived > &n_quat_b)
Calculates the Jacobian to convert an attitude represented in Euler angels (roll, pitch,...
Eigen::Matrix3d ie_Q_dr_domega(const Eigen::Vector3d &S_bgd, const Eigen::Matrix3d &ie_F_21, const Eigen::Matrix3d &ie_Dcm_b, const double &tau_s)
Submatrix 𝐐_35 of the system noise covariance matrix 𝐐
Eigen::Matrix3d G_GaussMarkov1(const Eigen::Vector3d &sigma2, const Eigen::Vector3d &beta)
Submatrix 𝐆_a of the noise input matrix 𝐆
Eigen::Matrix3d ie_Q_dr_df(const Eigen::Vector3d &S_bad, const Eigen::Matrix3d &ie_Dcm_b, const double &tau_s)
Submatrix 𝐐_34 of the system noise covariance matrix 𝐐
Eigen::Matrix3d Q_domega_psi(const Eigen::Vector3d &S_bgd, const Eigen::Matrix3d &b_Dcm_ien, const double &tau_s)
Submatrix 𝐐_51 of the system noise covariance matrix 𝐐
Eigen::Matrix3d Q_df_df(const Eigen::Vector3d &S_bad, const double &tau_s)
Submatrix 𝐐_44 of the system noise covariance matrix 𝐐
KeyedMatrix< Scalar, RowKeyType, ColKeyType, Eigen::Dynamic, Eigen::Dynamic > KeyedMatrixX
Dynamic size KeyedMatrix.
Eigen::Matrix3d ien_Q_dv_dv(const Eigen::Vector3d &S_ra, const Eigen::Vector3d &S_bad, const Eigen::Vector3d &S_rg, const Eigen::Vector3d &S_bgd, const Eigen::Matrix3d &ien_F_21, const double &tau_s)
Submatrix 𝐐_22 of the system noise covariance matrix 𝐐
Eigen::Matrix3d ie_Q_dr_psi(const Eigen::Vector3d &S_rg, const Eigen::Vector3d &S_bgd, const Eigen::Matrix3d &ie_F_21, const double &tau_s)
Submatrix 𝐐_31 of the system noise covariance matrix 𝐐
Eigen::Matrix3d G_RandomWalk(const Eigen::Vector3d &sigma2)
Random walk noise input matrix 𝐆
Eigen::Matrix3d n_Q_dr_dv(const Eigen::Vector3d &S_ra, const Eigen::Vector3d &S_bad, const Eigen::Vector3d &S_rg, const Eigen::Vector3d &S_bgd, const Eigen::Matrix3d &n_F_21, const Eigen::Matrix3d &T_rn_p, const double &tau_s)
Submatrix 𝐐_32 of the system noise covariance matrix 𝐐
KeyedMatrixX< T, KeyType, KeyType > e_noiseInput_G(const Eigen::Quaterniond &b_quat_p, const Eigen::Quaternion< T > &e_quat_b, bool accelBiases, bool gyroBiases, bool accelScaleFactor, bool gyroScaleFactor, bool accelMisalignment, bool gyroMisalignment)
Calculate the noise input matrix in Earth frame coordinates.
KeyedMatrixX< T, KeyType, KeyType > e_noiseScale_W(const Eigen::Vector3d &sigma2_ra, const Eigen::Vector3d &sigma2_rg, const Eigen::Vector3d &sigma2_bad, const Eigen::Vector3d &sigma2_bgd, const Eigen::Vector3d &sigma2_sad, const Eigen::Vector3d &sigma2_sgd, bool accelBiases, bool gyroBiases, bool accelScaleFactor, bool gyroScaleFactor)
Calculate the noise scale matrix in Earth frame coordinates.
Eigen::Matrix3d Q_df_dv(const Eigen::Vector3d &S_bad, const Eigen::Matrix3d &b_Dcm_ien, const double &tau_s)
Submatrix 𝐐_42 of the system noise covariance matrix 𝐐
Eigen::Matrix3d Q_psi_psi(const Eigen::Vector3d &S_rg, const Eigen::Vector3d &S_bgd, const double &tau_s)
Submatrix 𝐐_11 of the system noise covariance matrix 𝐐
Eigen::Matrix3d n_Q_dr_df(const Eigen::Vector3d &S_bad, const Eigen::Matrix3d &T_rn_p, const Eigen::Matrix3d &n_Dcm_b, const double &tau_s)
Submatrix 𝐐_34 of the system noise covariance matrix 𝐐
Eigen::Vector3d psd2BiasGaussMarkov(const Eigen::Vector3d &sigma2_bd, const Eigen::Vector3d &tau_bd)
u_bias^2 Power Spectral Density of the bias for a Gauss-Markov random process
Eigen::Matrix3d Q_domega_domega(const Eigen::Vector3d &S_bgd, const double &tau_s)
Submatrix 𝐐_55 of the system noise covariance matrix 𝐐
Eigen::Matrix3d n_Q_dr_psi(const Eigen::Vector3d &S_rg, const Eigen::Vector3d &S_bgd, const Eigen::Matrix3d &n_F_21, const Eigen::Matrix3d &T_rn_p, const double &tau_s)
Submatrix 𝐐_31 of the system noise covariance matrix 𝐐
Eigen::Matrix2d Q_gnss(const double &S_cPhi, const double &S_cf, const double &tau_s)
Submatrix 𝐐_66 of the system noise covariance matrix 𝐐
Eigen::Matrix3d ie_Q_dr_dv(const Eigen::Vector3d &S_ra, const Eigen::Vector3d &S_bad, const Eigen::Vector3d &S_rg, const Eigen::Vector3d &S_bgd, const Eigen::Matrix3d &ie_F_21, const double &tau_s)
Submatrix 𝐐_32 of the system noise covariance matrix 𝐐
Eigen::Matrix3d n_Q_dr_domega(const Eigen::Vector3d &S_bgd, const Eigen::Matrix3d &n_F_21, const Eigen::Matrix3d &T_rn_p, const Eigen::Matrix3d &n_Dcm_b, const double &tau_s)
Submatrix 𝐐_35 of the system noise covariance matrix 𝐐
Eigen::Matrix3d ien_Q_dv_psi(const Eigen::Vector3d &S_rg, const Eigen::Vector3d &S_bgd, const Eigen::Matrix3d &ien_F_21, const double &tau_s)
Submatrix 𝐐_21 of the system noise covariance matrix 𝐐
Eigen::Matrix3d ie_Q_dr_dr(const Eigen::Vector3d &S_ra, const Eigen::Vector3d &S_bad, const Eigen::Vector3d &S_rg, const Eigen::Vector3d &S_bgd, const Eigen::Matrix3d &ie_F_21, const double &tau_s)
Submatrix 𝐐_33 of the system noise covariance matrix 𝐐
Eigen::Matrix3d n_Q_dr_dr(const Eigen::Vector3d &S_ra, const Eigen::Vector3d &S_bad, const Eigen::Vector3d &S_rg, const Eigen::Vector3d &S_bgd, const Eigen::Matrix3d &n_F_21, const Eigen::Matrix3d &T_rn_p, const double &tau_s)
Submatrix 𝐐_33 of the system noise covariance matrix 𝐐
Eigen::Matrix3d ien_Q_dv_domega(const Eigen::Vector3d &S_bgd, const Eigen::Matrix3d &ien_F_21, const Eigen::Matrix3d &ien_Dcm_b, const double &tau_s)
Submatrix 𝐐_25 of the system noise covariance matrix 𝐐
Matrix which can be accessed by keys.