2. MATHEMATICAL FORMULATION

A causal non-linear filter reconstructs the present state given past measurements. An integrated navigation algorithm is a particular case of nonlinear filtering based on kinematics equations which, when using the Earth-Centred-Earth-Fixed (ECEF) frame as the reference frame, adopts the following form (see for example, España (Reference España2017)):

(1)$$\eqalign{\dot{{\bf P}}^{e} &= {\bf V}^{e} \cr \dot{{\bf V}}^{e} &= {\bf C}_{b}^{e}{\bf f}^{b} + \gamma^{e}\lpar {\bf P}^{e}\rpar - 2\lpar {\bf \Omega}^{e} \times {\bf V}^{e}\rpar \semicolon \; {\bf C}_{b}^{e} = \exp \lpar {\bf S}\lpar {\brtheta}_{be}\rpar \rpar \cr \dot{{\brtheta}}_{be} &= {\bromega}_{eb}^{e} + \displaystyle{{1}\over{2}} {\brtheta}_{be} \times {\bromega}_{eb}^{e} + \left(1 - \displaystyle{{{\rtheta}_{be} \sin {\rtheta}_{be}}\over{2\lpar 1 - \cos {\rtheta}_{be}\rpar }}\right){\mathop{{\brtheta}} \limits^{\smile}}_{be} \times \left({\mathop{{\brtheta}} \limits^{\smile}}_{be} \times {\bromega}_{eb}^{e}\right)\cr {\bromega}_{eb}^{e} &= {\bromega}_{ib}^{e} - {\bromega}_{ie}^{e} = {\bf C}_{b}^{e}{\bromega}_{ib}^{b} - {\bf \Omega}^{e}} $$

where: P^e, V^e are the vehicle's position and velocity in the ECEF frame; ${\brtheta}_{be}$ and ${\bf C}_{b}^{e}$ are, respectively, the vector angle and the rotation matrix relating the ECEF frame and the body frame; the specific force f^b and the angular rate ${\bromega}_{ib}^{b}$, both projected onto the body's frame, are gathered in the inertial magnitude vector ${\brmu} \triangleq \lsqb {\bf f}^{b\prime} {{\bromega}_{ib}^{b}}^{\prime}\rsqb ^{\prime}$, ((^′) stands for transpose), γ ^e(P^e) is the local gravity as a function of the vehicle's position, both in Earth Cartesian coordinates and Ω^e is the Earth angular rate. By using the Inertial Measurement Unit (IMU) inverse model and assuming additive disturbances ξ_μ (España, Reference España2017), ${\brmu}$ may be expressed as a function of its measured value ${\mathop{{\brmu}} \limits^{\frown}}$ as:

(2)$${\brmu} = {\cal M}\lpar {\mathop{{\brmu}} \limits^{\frown}}\semicolon \; {\bf p}_{i}\rpar = {\rxi}_{\rmu}\semicolon \; $$

The inertial as well as the exteroceptive instruments' models are characterised by a vector of unknown parameters ${\bf p} \triangleq \lsqb {{\bf p}_{i}}^{\prime}\ {{\bf p}_{e}}^{\prime}\rsqb ^{\prime}$ with assumed known expected values. The kinematics state vector ${\bf x} \triangleq \lsqb {\bf P}^{e \prime}\ {\bf V}^{e \prime}\ {{\rtheta}_{be}}^{\prime}\rsqb ^{\prime}$ and the instrument parameters vector p are compounded in the ASV defined as ${\brchi} \triangleq \lsqb {\bf x}^{\prime}\ {\bf p}^{\prime}\rsqb ^{\prime} \in {\open R}^{n}$. When p is modelled as a Brownian motion (Farrell, Reference Farrell2008), ${\brchi}$ may be modelled by the following stochastic equation, affine in the perturbations ξ(t):

(3)$$\eqalign{\dot{{\brchi}} &= \left[\matrix{ \dot{{\bf x}} \cr \dot{{\bf p}}}\right]= \overbrace{\left[\matrix{ {\bf f}_{kin} \lpar {\brchi}\semicolon \; {\mathop{{\brmu}} \limits^{\frown}}\lpar t\rpar \rpar \cr 0 }\right]}^{{\cal F}\lpar {\brchi}\semicolon t\rpar } + \overbrace{\left[\matrix{ {\bf B}_{kin} \lpar {\bf x}\rpar &0 \cr 0 & I \cr }\right]}^{{\bf B}\lpar {\bf x}\rpar } \overbrace{\left[\matrix{ {\brxi}_{\mu} \cr {\brxi}_{p} }\right]}^{{\rxi} \lpar t\rpar }\semicolon \; \cr {\mathop{\bf y}\limits^{\frown}}_{k} &= {\bf h}_{k}\lpar {\brchi} \lpar t_{k}\rpar \rpar + {\rm \eta}_{k}} $$

where h_k( · ) models the active exteroceptive instruments producing the measurement vector ${\mathop{y}\limits^{\frown}}_k$ at time t _k; η_k is a discrete time white disturbance with known diagonal covariance matrix R_k; the noise ξ_p models the sensor's parameters instabilities; ξ_μ and ξ_p are continuous white noises with assumed known Power Spectral Densities (PSD), respectively: S_μ, S_p.

For t ₀ as the starting time of an arbitrary trajectory stretch, the initial state ${\brchi} \lpar t_{0}\rpar $ in Equation (3) is a random vector with the first two moments $\hat{{\brchi}}_{0} = E\lcub {\brchi} \lpar t_{0}\rpar \rcub $, ${\bf P}_{0} = \hbox{cov}\lpar {\brchi} \lpar t_{0}\rpar \rpar $ provided by the navigation algorithm being assessed. When t ₀ is the initial navigation time, $\hat{{\brchi}}_{0}$, P₀ reflect the vehicle's alignment procedure and P₀ is normally assumed diagonal.

Given a reference solution of the nonlinear model Equation (3), denoted as ${\brchi}_{r}\lpar t\rpar $, we introduce the state deviations $\delta {\brchi} \lpar t\rpar \triangleq {\brchi} \lpar t\rpar - {\brchi}_{r} \lpar t\rpar $ and the innovations $\delta {\bf y}_{k} \triangleq {\mathop{\bf y} \limits^{\frown}}_{k} - {\bf h}_{k} \lpar {\brchi}_{r} \lpar t_{k}\rpar \rpar $, which, for smooth enough f_kim, h_k and small enough perturbations and initial state errors, may be modelled by the following time varying linear equations:

(4)$$\left\{\matrix{\delta \dot{{\brchi}} = {\bf A}\lpar {\brchi}_{r}\comma \; {\mathop{\mu} \limits^{\frown}}\rpar \delta {\brchi} + {\bf B}\lpar {\bf x}_{r}\rpar {\brxi}\semicolon \; \quad \delta {\brchi} \lpar t_{0}\rpar \sim \lpar \delta \hat{{\brchi}}_{0}\comma \; {\bf P}_{0}\rpar \hfill \cr \delta {\bf y}_{k} = {\bf H}_{k} \lpar {\brchi}_{r} \lpar t_{k}\rpar \rpar \delta {\brchi} \lpar t_{k}\rpar + {\rm \eta}_{k} \hfill \cr }\right. $$

where A( · ) and H_k( · ) are the partial derivatives matrices with respect to ${\brchi}$, respectively, of ${\cal F}\lpar {\brchi}\semicolon \; t\rpar $ and ${\bf h}_{k}\lpar {\brchi}\rpar $ in model Equation (3) evaluated along the reference trajectory.

When restricted to the kinematic state, from Equations (1), the deviation equations are given by (see, for example, España (Reference España2017), Equation 6.22).

(5)$$\eqalign{\delta \dot{{\bf x}} &= \left[\matrix{ 0 &{\bf I}_{3} &0 \cr \gamma_{P}^{e}\vert_{P^{e}} &-2{\bf S}\lpar {\bf \Omega}_{e}^{e}\rpar &- \hat{{\bf C}}_{{\bf b}}^{{\bf e}}{\bf S}\lpar {\mathop{f} \limits^{\frown}}^{\, e}\rpar \cr 0 &0 &-{\bf S}\lpar {\mathop{{\bromega}} \limits^{\frown}}_{ib}^{e}\rpar }\right]\left[\matrix{\delta {\bf P}^{e} \cr -\, -\, - \cr \delta {\bf V}^{e} \cr -\, -\, - \cr \delta {\brtheta}_{be}}\right]+ \left[\matrix{ 0 &0 \cr 0 &\hat{{\bf C}}_{{\bf b}}^{{\bf e}} \cr {\bf I} &0 }\right]\delta {\brmu} \cr &= {\bf A}_{kin} \lpar t\rpar \delta {\bf x} + {\bf B}_{kin} \lpar t\rpar \delta {\brmu}\semicolon \; \quad \delta {\bf x} \lpar t_{0}\rpar \sim \lpar \delta \hat{{\bf x}}_{0}\comma \; {\bf P}_{x0}\rpar \cr \delta {\brmu} &= {\bf B}_{p_{i}} \lpar {\mathop{{\brmu}} \limits^{\frown}}\rpar \delta {\bf p}_{i} + {\brxi}_{\mu}} $$

where: S : ℝ³ → ℝ^3×3 is the vector product matrix operator; $\gamma_{P}^{e}$ the Jacobian of the normal gravity with respect to P^e; $\delta {\bf P}^{e}\comma \; \delta {\bf V}^{e}\comma \; \delta {\brtheta}_{be}$, are the components of the kinematics state deviations vector and $\delta {\brmu}$ the IMU's deviation measurement vector, with ${\bf B}_{p_{i}} \lpar {\mathop{{\brmu}} \limits^{\frown}}\rpar $ the Jacobian of ${\cal M}$ (in Equation (2)) with respect to p_i.

The solution to the linear time varying differential Equation (4) in the interval [t _k t _k+1] between two consecutive exteroceptive measurement acquisition times, is given by (Farrell, Reference Farrell2008):

(6)$$\eqalign{\delta {\brchi} \lpar t_{k + 1}\rpar &= {\bf \Phi}\lpar t_{k + 1}\comma \; t_{k}\rpar \delta {\brchi} \lpar t_{k}\rpar + {\brvarepsilon}_{k}\semicolon \; {\brvarepsilon}_{k} \triangleq \vint_{t_{k}}^{t_{k + 1}} {\bf \Phi}\lpar t_{k + 1}\comma \; \tau\rpar {\bf B}\lpar \tau\rpar {\brxi}\lpar \tau\rpar d\, \tau \cr \dot{{\bf \Phi}}\lpar t\comma \; t_{0}\rpar &= {\bf A}\lpar t\rpar {\bf \Phi}\lpar t\comma \; t_{0}\rpar \semicolon \; {\bf \Phi}\lpar t_{0}\comma \; t_{0}\rpar = I} $$

where Φ( · , · ) ∈ ℝ^n×n is the state transition matrix of the time varying linear system Equation (4) and ${\brvarepsilon}_{k}$ is a centred uncorrelated random sequence such that:

(7)$$E\lsqb {\brvarepsilon}_{j} {\brvarepsilon}_{k}^{T}\rsqb = {\bf Q}_{k} \delta_{jk}\semicolon \; E\lsqb {\brvarepsilon}_{j} {\breta}_{k}^{T}\rsqb = 0\semicolon \; \quad \forall j\comma \; k $$

The positive definite covariance matrix Q_k is obtained by introducing the power spectral density (Starks and Woods, Reference Starks and Woods1994) S = diag(S_μ, S _p) in the integral expression of ${\brvarepsilon}_{k}$ given in Equation (6) (see España (Reference España2017) for details) and δ_jk stands for the Kronecker delta. Denoting the non-singular matrix F_k = Φ(t _k+1, t _k) and $\delta {\brchi}_{k} = \delta {\brchi}\lpar t_{k}\rpar $, with Equation (6), the discrete time models for the ASV and the innovation are rewritten as:

(8)$$\left\{\matrix{\delta {\brchi}_{k + 1} = {\bf F}_{k} \delta {\brchi}_{k} + \varepsilon_{k}\semicolon \; \delta {\brchi}_{0} \sim \lpar \delta \hat{{\brchi}}_{0}\comma \; {\bf P}_{0}\rpar \hfill \cr \delta {\bf y}_{k} = {\bf H}_{k} \delta {\brchi}_{k} + {\rm \eta}_{k} \hfill \cr }\right. $$

The EKF refers the state deviations to the a priori estimate $\hat{{\brchi}}_{k + 1}^{-}$ calculated at the end of the k-th interval by numerical integration of model Equation (3) after substitution of all random variables by their expected values, that is: ξ → 0; ${\brchi}\lpar t_{k}\rpar \rightarrow \hat{{\brchi}}_{k}$ -the last one being the previous a posteriori estimate obtained at the beginning of the k-th interval. This procedure, first introduced by Jazwinski (Reference Jazwinski1970), is known as the Certainty Equivalence Principle. Once the next measurement becomes available, the a posteriori estimate $\hat{{\brchi}}_{k + 1}$ is obtained, updating the prediction $\hat{{\brchi}}_{k + 1}^{-}$ by adding the innovation: ${\mathop{\bf y} \limits^{\frown}}_{k + 1} - {\bf h}_{k} \lpar \hat{{\brchi}}_{k + 1}^{-}\rpar $ multiplied by the Kalman gain calculated as (Anderson and Moore, Reference Anderson and Moore1979):

(9)$${\bf K}_{k} = {\bf P}_{k}^{-} {\bf H}_{k}^{T} \lpar {\bf H}_{k} {\bf P}_{k}^{-} {\bf H}_{k}^{T} + {\bf R}_{k}\rpar ^{-1} = {\bf P}_{k} {\bf H}_{k}^{T} R_{k}^{-1} $$

where ${\bf P}_{k}^{-} = E\lpar {\brchi}_{k} - \hat{{\brchi}}_{k}^{-}\rpar \lpar {\brchi}_{k} - \hat{{\brchi}}_{k}^{-}\rpar ^{T}$ and ${\bf P}_{k} = E\lpar {\brchi}_{k} - \hat{{\brchi}}_{k}\rpar \lpar {\brchi}_{k} - \hat{{\brchi}}_{k}\rpar ^{T}$ are, respectively, the a priori and the a posteriori covariances of the state error modelled by Equation (8). We introduce the a priori and a posteriori information matrices: ${\cal I}_{k}^{-} \triangleq \lpar {\bf P}_{k}^{-}\rpar ^{-1}$, ${\cal I}_{k} \triangleq {\bf P}_{k}^{-1}$. As shown in Anderson and Moore (Reference Anderson and Moore1979) or Jazwinski (Reference Jazwinski1970), both matrices may be determined with the next recursive equations starting from ${\cal I}_{0}^{-1} = {\bf P}_{0} \gt 0$.

(10)$$\matrix{a\rpar \, {\bf P}_{k + 1}^{-} = {\bf F}_{k}{\bf P}_{k}{\bf F}_{k}^{T} + {\bf Q}_{k}\semicolon \; \hfill & \leftarrow \hbox{covariance propagation} \hfill \cr b\rpar \, {\cal I}_{k + 1} = {\cal I}_{k + 1}^{\, -} + {\bf H}_{k + 1}^{T} {\bf R}_{k + 1}^{-1} {\bf H}_{k + 1}\semicolon \; \hfill & \leftarrow \hbox{information update} \hfill \cr } $$

From Equations (10), the current state accuracy at any time t _k is the consequence of two opposed effects. The first one, called “disturbability” by Bryson (Reference Bryson1978), concerns the growth of the state uncertainty due to the disturbance covariance noise matrix Q_k (Equation (10a)). The second one, called “estimability” by Baram and Kailath (Reference Baram and Kailath1988) regards the growth of the information matrix produced by the new measurement acquired at time t _k+1 (Equation (10b). Both effects depend on the Jacobians A( · ), H( · ) and B( · ) evaluated over the reference trajectory. As such, the accuracy of the ASV estimate depends on: (a) the performance of the onboard instruments (through the inertial sensor's PSD S and external measurement noise R_k), (b) the initial state errors covariance P₀ and (c) the vehicle's trajectory itself (Giribet et al., Reference Giribet, Mas and Moreno2018).

Since the components of $\delta {\brchi}_{k}$, δy_k and those of the corresponding driving noises are expressed in different physical units, their magnitudes are not directly comparable. Thus, for D₀≜diag(P₀) and R₀ assumed diagonal, we proceed to normalise (“de-dimensionalise”) model Equation (8) through the following change of variables (Krener and Kayo, Reference Krener and Kayo2009):

(11)$$\delta {\brchi}_{k} \rightarrow {\bf D}_{0}^{-1/2} \delta {\brchi}_{k}\semicolon \; \delta {\bf y}_{k} \rightarrow {\bf R}_{0}^{-1/2} \delta {\bf y}_{k} $$

With variable change Equation (11), matrices in Equation (8) are transformed as: ${\bf F}_{k} \rightarrow {\bf D}_{0}^{1/2}{\bf F}_{k}{\bf D}_{0}^{1/2}$; ${\bf H}_{k} \rightarrow {\bf R}_{0}^{-1/2} {\bf H}_{k} {\bf D}_{0}^{1/2}$. Thus, the new normalised state deviations' covariance transforms as: ${\bf P}_{k} \rightarrow {\bf D}_{0}^{-1/2}{\bf P}_{k}{\bf D}_{0}^{-1/2}$; ∀ k, while the output additive error noise covariance as: ${\bf R}_{k} \rightarrow {\bf R}_{0}^{-1/2}{\bf R}_{k}{\bf R}_{0}^{-1/2}$; ∀ k. Moreover, with ${\bf Q}_{k} \rightarrow {\bf D}_{0}^{-1/2}{\bf Q}_{k} {\bf D}_{0}^{-1/2}$, a simple substitution shows that the new normalised matrix P_k still satisfies the recursive Equations (10). We call ${\cal E} \triangleq \lcub {\bf e}^{i} \colon i = 1\comma \; 2\comma \; \ldots\comma \; n\rcub $ the canonical base in which the physical but dimensionless state is represented after transformation Equation (11).

${\cal I}_{k}$, a real symmetric positive definite matrix for any k, may be decomposed into a diagonal form, such as ${\cal I}_{k} = {\bf V}_{k} \Lambda_{k} {\bf V}_{k}^{T}$ where, $\Lambda_{k} = {diag}\lpar \lambda_{k}^{1}\comma \; \lambda_{k}^{2}\comma \; \ldots\comma \; \lambda_{k}^{n}\rpar $ is built with the ordered set of ${\cal I}_{k}$'s eigen-values: $\lcub \lambda_{k}^{1} \geq \lambda_{k}^{2} \geq \cdots \geq \lambda_{k}^{n} > 0\rcub $ and ${\bf V}_{k} = \lsqb {\bf v}_{k}^{1}\ \vert\ {\bf v}_{k}^{2}\ \vert\ \ldots\ \vert\ {\bf v}_{k}^{n}\rsqb \in {\open R}^{nxn}$ is the matrix with, as columns, the set of ${\cal I}_{k}$'s unit orthogonal right eigen-vectors. In what follows, it is assumed that there exists $0 < \underline{p} < \bar{p} < \infty$ such that for all $t_{k}\ \underline{p} < \lambda_{k}^{\prime} < \bar{p}$, i = 1, …, n. This ensures that there is no numerical divergence on recursion Equations (10) and also that there are no purely deterministic directions in the state space (see next section).

3. RELATIVE SENSABILITY: SUBSPACES AND METRICS

Assuming the ${\bf v}_{k}^{i}$ expressed in the canonical base ${\cal E}$ in which the normalised augmented physical state model Equation (8) is represented, we introduce the transformation $\delta {\brchi}_{k} \rightarrow \delta {\bf z}_{k}$ in ℝⁿ defined as:

(12)$$\delta {\bf z}_{k} = {\bf V}_{k}^{T}\delta {\brchi}_{k}\semicolon \; $$

The component $\delta z_{k}^{i}$ of δz_k is the projection of the state deviation vector $\delta {\brchi}_{k}$ (“physical” normalised coordinates) over the i-th ${\cal I}_{k}$'s eigen-vector. At each instant t _k the covariance matrix of the transformed vector is:

(13)$$\eqalign{\hbox{Cov}\lpar \delta {\bf z}_{k}\rpar &= {\bf V}_{k}^{T}\ E\lcub \delta {\brchi}_{k} \delta {{\brchi}_{k}}^{T}\rcub {\bf V}_{k} \cr &= {\bf V}_{k}^{T} {\bf P}_{k} {\bf V}_{k} = \lpar {\bf V}_{k}^{T} {\cal I}_{k} {\bf V}_{k}\rpar ^{-1} = \Lambda_{k}^{-1}} $$

From Equations (12) and (13) the transformed state deviation components are orthogonal random variables satisfying:

(14)$$\matrix{i\rpar \, E\lcub \delta z_{k}^{i} \delta z_{k}^{i}\rcub = 0 \quad \forall i \ne j \hfill \cr ii\rpar \, E\lcub \lpar \delta z_{k}^{i}\rpar^{2}\rcub = 1/\lambda_{k}^{i} \hfill \cr iii\rpar \comma \; \ E\lcub \Vert \delta {\bf z}_{k}\Vert^{2}\rcub = E\lcub \Vert \delta {\brchi}_{k}\Vert^{2}\rcub = \sum\nolimits_{i} 1/\lambda_{k}^{i} = {tr}\lpar {\bf P}_{k}\rpar \hfill \cr } $$

Each standard deviation $\sqrt{1/\lambda_{k}^{i}}$ being the state uncertainty along the eigen-direction ${\bf v}_{k}^{i}$, $\sqrt{{tr}\lpar {\bf P}_{k}\rpar }$ is a measure of the instantaneous “total state uncertainty”. Symmetrically, $\sqrt{{tr}\lpar {\cal I}_{k}\rpar }$ is seen as a measure of the “total state accuracy”. Given the ordering of the $\lambda_{k}^{\prime}$'s, the state space direction best estimated (least variance) is the first principal component (pc) ${\bf v}_{k}^{1}$ of the information matrix ${\cal I}_{k}$ with $\hbox{Cov}\lpar \delta z_{k}^{1}\rpar = 1/\lambda_{k}^{1}$; the next best estimated is ${\bf v}_{k}^{2}$ and so on until ${\bf v}_{k}^{n}$.

We now consider the sets of sequentially embedded subspaces ${\cal V}_{k}^{i} \triangleq {span}\lcub {\bf v}_{k}^{1}\comma \; {\bf v}_{k}^{2}\comma \; \ldots\comma \; {\bf v}_{k}^{i}\rcub \lpar {\cal V}_{k}^{i + 1} \supset {\cal V}_{k}^{i}\rpar $ and introduce the following ratios monotonously increasing with i towards r ⁿ = 1.

(15)$$r_{k}^{i} = \sqrt{\sum_{j = 1}^{i} \left.\lambda_k^{j}\right/\sum_{j = 1}^{n} \lambda_{k}^{j}} \colon i = 1\comma \; 2\comma \; \ldots\comma \; n $$

If, for a given i ≤ n, r ^t ≈ 1, it means that the subspace ${\cal V}_{k}^{\prime}$ contains the pcs that concentrates the major part of the total accuracy of the estimator. Given this, we introduce:

Definition 1: For a threshold 0 < ν< 1 empirically chosen close to 1, we denote nr the first index i in Equation (15) such that $r_{k}^{nr} \geq \nu$ and call $S_{k}^{s} \equiv {\cal V}_{k}^{nr}$ the practically sensable (ps) subspace and $S_{k}^{NS} \equiv \lpar S_{k}^{S}\rpar ^{\perp}$ the practically non-sensable (pns) subspace to its orthogonal complement.

By definition, the random components of δ z _i over $S_{k}^{S}$ are the ps ones. Since ${\open R}^{n} = S_{k}^{S} \oplus S_{k}^{NS}$, on any t _k, each element e_i of the canonical (physical) base may be decomposed into its orthogonal projections π_s(e_i) _k and π_ns(e_i) _k, respectively, over $S_{k}^{S}$ and $S_{k}^{NS}$ as:

(16)$${\bf e}_{i} = \sum\limits_{{\bf v}_{j} \in S_{k}^{s}} \langle{\bf e}_{i}\comma \; {\bf v}_{j}\rangle {\bf v}_{j} + \sum\limits_{{\bf v}_{l} \in S_{k}^{ns}} \langle{\bf e}_{i}\comma \; {\bf v}_{l}\rangle {\bf v}_{l} \triangleq \pi_{s} \lpar {\bf e}_{i}\rpar _{k} + \pi_{ns} \lpar {\bf e}_{i}\rpar _{k} $$

Definition 2: $s_{k}^{i} \triangleq \Vert \pi_{s} \lpar {\bf e}_{i}\rpar_{k}\Vert $ is the practical sensability instantaneous measure of the ASV's i-th component.

Some practical considerations on the choice of the threshold ν are in order. It is assumed that at least one component of the ASV is ps (otherwise the instrumentation should be reviewed) so, whenever the $\lambda_{k}^{i}$'s are similar $\lpar \lambda_{ki}^{i} \approx \bar{\lambda}_{k}\comma \; \forall i\rpar $ one expects that all the ASV components be ps (that is: $S_{k}^{s} = {\cal V}_{k}^{n}$). Now, since in this case $r_{k}^{i} \approx \sqrt{i/n}$, it needs to be ν ∈ (1 − 1/n, 1), or else, one may have $r_{k}^{n - 1} \gt \nu$ meaning that the last component is pns, which is contrary to the assumption. Thus, for a small enough ε empirically chosen, we propose the dimensionally dependent choice of $\nu = \sqrt{1 - 1/n + \varepsilon} \lt 1$.

Notice that decomposition Equation (16) is generically time variant, which means that some eigen-directions may go in and out of the subspace $S_{k}^{S}$. As such, its associated measure may change with time depending on previous data. This motivates quantifying the available information embedded in a given vehicle's trajectory segment, which is the subject of the next section. Our point of view differs here once more with respect to that of Tang et al. (Reference Tang, Wu, Wu, Wu, Hu and Shen2009) in the sense that we search to study, segment by segment, the actual vehicle's trajectory.

4. OBSERVABILITY AND THE EXCITABILITY METRIC

Contrary to the sensability metric which is evaluated at each time t _k, we now seek to assess the actual information conveyed by the innovations along a given time interval. The measure we look for is not intended to depend on the instrumentation performance but to meet the practical need of quantifying the impact of vehicles' manoeuvres on the estimability of the ASV, for a given instrument configuration.

The analysis is performed by first synthesising a deterministic trajectory with corresponding measurements signals (inertial and exteroceptive). The method, proposed by Giribet et al. (Reference Giribet, España and Miranda2007), produces the smooth B-spline exactly satisfying the kinematics Equations (1) that best fits the flight data in the a mean square sense. The measurements are calculated with Equation (2) assuming perfect instruments, that is: ξ_μ = 0, ${\brxi}_{p} = 0\ \forall t$, ${\rm \eta}_{k} = 0 \forall t_{k}$ and also ${\brvarepsilon}_{k} = 0$ in Equation (6). The synthetic trajectories and measurements are then used as references to construct the linearized state deviations and innovation models in Equations (4), (5) and (8).

4.1. Observability analysis

Given the above assumptions, the innovations on the set of time instants τ = {t ₀, t ₁, …, t _N} modelled by Equation (8) may be expressed as a linear function of the initial state deviation as:

(17)$${\bf Y}_{\tau} \lpar \delta {\bf x}_{0}\rpar = \left[\matrix{ {\bf y} \lpar t_{0}\rpar \cr {\bf y} \lpar t_{1}\rpar \cr \cdots \cr y \lpar t_{N - 1}\rpar }\right]= \left[\matrix{ {\bf H}_{0} \cr {\bf H}_{1} {\bf F}_{0} \cr \cdots \cr {\bf H}_{N - 1} {\bf F}_{N - 2} \cdots {\bf F}_{1} {\bf F}_{0} }\right]\delta {\brchi}_{0} \triangleq {\bf O}_{\tau} \delta {\brchi}_{0} $$

The matrix O_τ ∈ ℝ^{(pN) xn} was called the extended observability matrix by Moore (Reference Moore1981). If a null linear combination were to exist among the n columns of O_τ a whole non-zero subspace of the initial deviations state-space would be mapped into the null innovation signal. Accordingly, the largest subspace of ℝⁿ contained in the kernel of O_τ is the unobservable subspace of the system Equation (8) within the interval τ. When the unobservable subspace is reduced to the origin, the system Equation (8) is said to be completely observable. Observability is thus a binary condition stating whether or not the innovations carry total information about the initial state deviations. Unfortunately, this does not give us a hint of which state components may be better determined and, more importantly, in what amount.

4.2. Principal components of the innovation signal

We now introduce the Observability Gramian for the interval τ: ${\bf M}_{\tau} \triangleq {\bf O}_{\tau}^{T}{\bf O}_{\tau} \in {\open R}^{n \times n}$. Being a symmetric non-negative definite matrix, there exists a set $B_{\tau} = \lcub {\bf u}_{\tau}^{i} \in {\open R}^{n} \colon i = 1\comma \; 2\comma \; \ldots\comma \; n\rcub $ of unitary, mutually orthogonal eigenvectors associated with the ordered set $\lcub \sigma_{\tau}^{1} \geq \sigma_{\tau}^{2} \geq \ldots \geq \sigma_{\tau}^{n}\rcub $ of non-negative eigen-values (all positives only if system Equation (8) is completely observable, that is: M_τ is non-singular). With the elements of B _τ expressed in coordinates of the canonical base, the orthogonal matrix ${\bf U}_{\tau} = \lsqb {\bf u}_{\tau}^{1}\ \vert\ {\bf u}_{\tau}^{2}\ \vert\ \ldots\ \vert\ {\bf u}_{\tau}^{n}\rsqb \in {\open R}^{ncn}$ leads to the similarity transformation ${\bf M}_{\tau} = {\bf U}_{\tau}^{T}{\bf \Sigma}_{\tau}{\bf U}_{\tau}$ for ${\bf \Sigma}_{\tau} \triangleq {diag}\lcub \sigma_{\tau}^{1}\comma \; \sigma_{\tau}^{2}\comma \; \ldots\comma \; \sigma_{t}^{n}\rcub $. Using the fact that $\sum\nolimits_{i} {\bf u}_{\tau}^{i} {\bf u}_{\tau}^{iT} = I_{n}$, we write:

(18)$${\bf O}_{\tau} = {\bf O}_{\tau} \left(\sum\limits_{i} {\bf u}_{\tau}^{i} {\bf u}_{\tau}^{iT}\right)= \sum\limits_{i} {\bf o}_{\tau}^{i} {\bf u}_{\tau}^{iT} $$

The set of responses to the initial state deviations ${\bf u}_{\tau}^{i}$: ${\bf o}_{\tau}^{i} \triangleq {\bf O}_{\tau} {\bf u}_{\tau}^{i} = {\bf Y}_{\tau} \lpar {\bf u}_{\tau}^{i}\rpar $, i = 1, …, n, are called the innovation signal principal components (ispc) in τ. As can be shown, they are mutually orthogonal (that is: ${\bf o}_{\tau}^{iT} {\bf o}_{\tau}^{j} = \sigma_{\tau}^{j} \delta_{ij}$) and such that $\Vert {\bf o}_{\tau}^{i}\Vert^{2} = \sigma_{\tau}^{i}$. Moreover, for any initial state $\delta {\brchi}_{0} = \sum\nolimits_{i} \alpha_{i} {\bf u}_{\tau}^{i}$ represented in B _τ coordinates, the innovation signal is expressed by the same linear combination of the ispcs, that is: ${\bf Y}_{\tau} \lpar \delta \chi_{0}\rpar = \sum_{i} \alpha_{i} {\bf o}_{\tau}^{i}$. On the other hand, for a given innovation signal over an interval τ, the components on B _τ of the initial state may be recovered through: $\alpha_{i} = \lpar {\bf Y}_{\tau}^{T} {\bf o}_{\tau}^{i}\rpar /\sigma_{\tau}^{i}$ provided that $\sigma_{\tau}^{i} \, {\ne}\, 0$. This just restates the fact that an unobservable subspace exists in τ whenever $\sigma_{\tau}^{n} = 0$.

By using the orthogonality of the ${\bf 0}_{\tau}^{i}$ s, one shows that the total energy of the output signal over τ, given by: $E_{\tau} \lpar \delta {\brchi}_{0}\rpar \triangleq \Vert {\bf Y}_{\tau} \lpar \delta {\brchi}_{0}\rpar \Vert^{2} = \sum\nolimits_{i} \alpha_{i}^{2} \Vert {\bf o}_{\tau}^{i}\Vert^{2}$, which may also be decomposed into a linear combination of the individual ispc's energies: $E_{\tau} \lpar {\bf u}_{\tau}^{\prime}\rpar = \Vert {\bf o}_{\tau}^{\prime}\Vert^{2} = \sigma_{\tau}^{i}$, with, as coefficients, the squares of the initial state components in B _τ. Clearly, the combined energy of all the ispcs within τ is tr(M_τ).

4.3. An excitability metric

We now consider the mapping sending any point of the (unit) n-hypersphere S ⁿ into ℝⁿ : e_τ(v) = E _τ(v)v with ‖v‖ = 1. Its image e_τ(S ⁿ) describes an n-hyper-ellipsoid E ⁿ with axes $\sigma_{i}^{\tau} {\bf u}_{\tau}^{i}$; i = 1, …, n. The radius vector to each point on E ⁿ measures the sensitivity of the innovation's energy to an initial state deviation in that particular direction. The maximum sensitivity is $\sigma_{\tau}^{1}$ attained in the direction of ${\bf u}_{\tau}^{1}$.

In practice, for a given vehicle's trajectory and time interval τ, one may expect that the innovation's energy will be almost insensitive to some directions in S ⁿ. This motivates establishing a criterion to decide which directions contribute to the innovations and which do not.

Considering that tr(M_τ)/n is the average energy induced by the ${\bf u}_{\tau}^{i}s$, we introduce the relative excitability index of the direction v on S ⁿ:

(19)$$S_{\tau}\lpar {\bf v}\rpar \triangleq \displaystyle{{n}\over{{tr}\lpar {\bf M}_{\tau}\rpar }} \sum\nolimits_{i} \alpha_{i}^{2}\sigma_{\tau}^{i}\semicolon \; \quad \Vert {\bf v}\Vert^{2} = \sum\nolimits_{i} \alpha_{i}^{2} = 1 $$

Now, if for a sufficiently small number η, empirically chosen, there exists an index ne ∈ {1, …, n} such that:

(20)$$S_{\tau}\lpar {\bf u}_{\tau}^{ne}\rpar \leq {\rm \eta}\semicolon \; 0 \lt n \ll 1\semicolon \; \ {for some ne} $$

then, that means that the contribution of the components ${\bf u}_{\tau}^{ne}\comma \; \ldots\comma \; {\bf u}_{\tau}^{n}$ to the total energy tr(M_τ) may be neglected. This establishes a partition in the orthogonal set $\lcub {\bf u}_{\tau}^{i}\rcub $, which in turn determines the orthogonal complementary subspaces:

(21)$$\eqalign{S_{\tau}^{O} & \equiv {span}\lcub {\bf u}_{\tau}^{i}\semicolon \; i \lt {ne}\rcub \cr S_{\tau}^{{NO}} &= S_{\tau}^{O \perp} \equiv {span}\lcub {\bf u}_{\tau}^{i}\semicolon \; i \geq {ne}\rcub } $$

Definition 3: $S_{\tau}^{O}$ is called the practically observable subspace and the practically non-observable one.

Since ${\open R}^{n} = S_{\tau}^{O} \oplus S_{\tau}^{{NO}}$, as in Equation (16), each element eⁱ of the canonical base ${\cal E}$ is decomposed into its orthogonal projections $\pi_{\tau}^{O} \lpar {\bf e}^{i}\rpar $ and $\pi_{\tau}^{{NO}}\lpar {\bf e}^{i}\rpar $ respectively over $S_{\tau}^{O}$ and $S_{\tau}^{{NO}}$:

(22)$${\bf e}^{i} = \sum\limits_{{\bf u}_{\tau}^{j} \in S_{\tau}^{O}} \langle{\bf e}^{i}\comma \; {\bf u}_{\tau}^{j}\rangle {\bf u}_{\tau}^{j} + \sum_{{\bf u}_{\tau}^{j} \in S_{\tau}^{NO}} \langle{\bf e}^{i}\comma \; {\bf u}_{\tau}^{j}\rangle {\bf u}_{\tau}^{j} \triangleq \pi_{\tau}^{O} \lpar {\bf e}^{i}\rpar + \pi_{\tau}^{{NO}} \lpar {\bf e}^{i}\rpar $$

Definition 4: $e_{\tau}^{i} \triangleq \Vert \pi_{\tau}^{O} \lpar {\bf e}^{i}\rpar \Vert$ is the practical excitabilty metric of the ASV's i-th component over the interval τ.

Notice that (contrary to Shen et al. (Reference Shen, Xia, Wang, Neusypin and Proletarsky2018)) no a priori claim on the conditioning number of the local observability matrix O_τ has been invoked. From the definition it naturally follows that, whenever rang(O_τ) < n, there exist a linear subspace of the ASV state space with zero practical excitabilty metric.

5. SUBORBITAL ROCKET EXPERIMENTAL DATA ANALYSIS

The above concepts were applied to a CONAE navigation and control experimental payload on board a Brazilian VS30 sounding rocket having a tactical IMU (Systron Donner Motion-Pack 3 accelerometers and three gyros) sampled at 100 Hz and a GPS receiver delivering ECEF position at 1 Hz.

5.1. The trajectory description

An analytic trajectory defined for any time t was synthesised (by means of the approach proposed by Giribet et al. (Reference Giribet, España and Miranda2007) mentioned above) to fit the actual flight data. Figure 1 shows the X components, aligned with the rocket axis, of the synthetic specific force and angular rate, and evidences the different flight stages.

Figure 1.

Axial specific force and angular velocity.

The vehicle stood on the launching pad until t = 18 s. During propulsion (18 s < t < 46 s) it attained a maximum acceleration of 110 m/s² while being spun aerodynamically. At t = 46 s, the thrust was turned off, changing the sign of the axial acceleration due to residual air friction. An almost free flight stage (nearly null specific force) started at t = 60 s above 40 km above sea level. At t = 73 s a de-spin mechanism was activated reducing the axial angular rate by a half. The atmospheric reinsertion phase, not shown in the figure, started a few seconds later.

5.2. The ASV deviation equations

The ASV ${\brchi} \in {\open R}^{21}$ in Equation (3) includes the kinematic state x ∈ ℝ⁹ (referred to ECEF) and the IMU's parameters vector p_i ∈ ℝ¹² (six biases plus six scale factors). Also ξ ∈ ℝ²¹ while ${\breta}_{k} \in {\open R}^{3}$ is the GPS position error measurement. The ASV deviation vector in Equation (4) is defined as (sub-indices a and b stand, respectively, for accelerometer and gyro):

(23)$$\eqalign{\delta {\brchi} &\triangleq \lsqb \delta {\bf P}^{eT}\ \delta {\bf V}^{eT}\ \delta {\brtheta}_{be}^{T}\ \delta {\bf p}_{i}^{T}\rsqb ^{T} \in {\open R}^{21}\semicolon \; \cr \delta {\bf p}_{i} &= \lsqb \delta {\bf b}_{g}^{T}\ \delta {\bf b}_{a}^{T}\ \delta {\bf s}_{g}^{T}\ \delta {\bf s}_{a}^{T}\rsqb ^{T} \in {\open R}^{12}} $$

The specific time varying matrices in Equations (4) and (5), given below, are used as stated in Section 2 to obtain the stochastic discrete time Equation (8) used in the EKF fusion filter.

(24)$${\bf A}\lpar t\rpar = \left[\matrix{{c\colon c} {\bf A}_{kim} \vert {\bf B}_{kin}\ {\bf B}_{{\bf p}_{i}} \cr ---- ---- \cr {{\bf 0}_{12 \times 21}} \cr }\right]\semicolon \; {\bf B}\lpar t\rpar = {\bf B}_{{\bf p}_{i}}\semicolon \; {\bf H}_{k} = \left[\matrix{ {\bf I}_{3 \times 3} & {\bf 0}_{3 x 18} }\right]$$

5.4. Analysis of the sensability metric

As seen before, the GPS position direct measure ensures almost perfectly excitable position and velocity vectors all along the trajectory. This induces a high sensability metric of the position from the very beginning and a growing tendency in the sensability of the velocity. For the assumed value u = 0·985 in Definition 1, Figure 3(a) shows this fact for the ECEF x-components, while Figure 3(b) demonstrates how, being a measure of the available information rate, the sensabilty anticipates changes of the estimates' 2σ error band. Actually, the former is a sort of “derivative” of the latter. As such, a deep valley in the velocity sensability before t = 5 sec. induces a growth of the estimation error band, while, close to t = 12 s, the metric increase induces an error reduction. Finally, after t = 55 s the metric stabilises at its highest value (~eq1) in correspondence to the lowest estimation error band of the velocity.

An exciting trajectory does not necessarily imply high sensability, but it is a required condition besides high performance instruments. Figure 2 shows that the attitude (θ_be) is highly exciting on trajectory segments τ₁, τ₂, however, as seen in Figure 4(a), the sensability of q _x and q _y only grow after blast-off (t = 19 s). While the vehicle is at rest the attitude excitability is the consequence of two vectors potentially measured by the IMU: the Earth angular rate and gravity. However, the random walk noise and bias instability levels (see España (Reference España2017)) of the tactical IMU's gyro used prevent measuring the first one and thus to take full advantage of a high excitability. Notice that, under these conditions, the IMU works as a mere inclinometer and thus, as shown in Figure 4(a), the θ_be components shall be more sensable the larger their projections are over the local tangent plane.

Figure 2.

Excitability metric on intervals τ₁, τ₂ and τ₃.

Figure 3.

(a) Sensability metrics of position and velocity on the ECEF x-axis; (b) 2-σ bounds estimation errors over the x-axis.

Figure 4.

(a) Sensability metric of the attitude vector θ_be; (b) Corresponding 2-σ estimation errors bounds.

On the other hand, the propulsion ignition induces a boost of inertial measurements (acceleration and angular rate) within the sensitivity range of the IMU inducing a fast and sustained growth the sensability of θ_be during τ₂, accompanied by a reduction of its error bandwith (Figure 4(b)). At last, during free fall (τ₂ : t ≥ 40 < /xref > s), a steady descent of the attitude sensability corresponds to a smooth increase of its error bandwidth (Figure 4(b)).

For similar reasons the IMU parameters (accelerometer and gyro biases and scale factors) turn out to be weakly sensable too. Only the y-component bias appears to be sensable but only during an interval so short in τ₂, that the filter is unable to gather enough information so as to improve its a priori error bound (see Figure 5(a) and 5(b)). The above result suggests that for the given trajectory and instrumentation it may be futile to try to improve the a priori instrument calibration during flight.

Figure 5.

(a) Sensability metric of x-component of the gyro's bias and (b) its corresponding estimation error's 2-σ bounds.