1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
//! Geometry observability and residual-validation classification.
/// Observability and validation tier for an estimation geometry.
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub enum ObservabilityTier {
/// The design rank is below the parameter count, so at least one parameter
/// is not observable.
RankDeficient,
/// The design is full rank, but has no residual degrees of freedom.
ZeroRedundancy,
/// The design is full rank with residual degrees of freedom, but exceeds a
/// configured condition-number or GDOP cutoff.
Weak,
/// The design is full rank and does not exceed the configured cutoffs.
Nominal,
}
/// Geometry observability and covariance-validation diagnostics.
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct GeometryQuality {
/// Tier assigned from rank, redundancy, condition number, GDOP, and prior
/// availability.
pub tier: ObservabilityTier,
/// Observation redundancy, defined as `n_obs - n_params`.
pub redundancy: i32,
/// Rank of the design matrix used by the solve.
pub rank: usize,
/// Condition number of the design matrix, computed as `sigma_max /
/// sigma_min` from its singular values.
pub condition_number: f64,
/// Geometric dilution of precision for the solved state.
pub gdop: f64,
/// Whether residual-based RAIM can test the solve.
pub raim_checkable: bool,
/// Whether residuals or a valid propagated prior can validate the
/// covariance bound.
pub covariance_validated: bool,
}
/// Configurable cutoffs for [`classify`].
///
/// The default uses `cond_cutoff = 1.0e8` and `gdop_cutoff = 10.0`.
/// The condition-number cutoff follows the standard first-order linear-system
/// error amplifier, where `kappa(H) * eps` approximates relative numerical
/// sensitivity. In `f64`, `1.0e8` is far above ordinary scaling noise but still
/// below the singular-value rank threshold used by the least-squares covariance
/// path. The GDOP cutoff sits in the common GNSS screening band around 6 to 20;
/// `10.0` marks a geometry before a DOP-only projection becomes very large.
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct GeometryQualityThresholds {
/// Maximum accepted singular-value condition number for a full-rank solve
/// with positive redundancy.
pub cond_cutoff: f64,
/// Maximum accepted GDOP for a full-rank solve with positive redundancy.
pub gdop_cutoff: f64,
}
impl Default for GeometryQualityThresholds {
fn default() -> Self {
Self {
cond_cutoff: 1.0e8,
gdop_cutoff: 10.0,
}
}
}
/// Classify geometry observability and covariance validation from scalar
/// diagnostics.
///
/// `rank` is compared to `n_params`. `redundancy` is `n_obs - n_params`.
/// `condition_number` must be the singular-value ratio of the design matrix,
/// not the condition number of the normal matrix. A non-finite condition number,
/// GDOP, or cutoff is treated as exceeding the corresponding cutoff for
/// full-rank positive-redundancy cases.
pub fn classify(
rank: usize,
n_params: usize,
redundancy: i32,
condition_number: f64,
gdop: f64,
has_valid_prior: bool,
thresholds: GeometryQualityThresholds,
) -> GeometryQuality {
let (tier, raim_checkable, covariance_validated) = if rank < n_params {
(ObservabilityTier::RankDeficient, false, false)
} else if redundancy == 0 {
(ObservabilityTier::ZeroRedundancy, false, has_valid_prior)
} else if redundancy >= 1
&& (exceeds_cutoff(condition_number, thresholds.cond_cutoff)
|| exceeds_cutoff(gdop, thresholds.gdop_cutoff))
{
(ObservabilityTier::Weak, true, true)
} else {
let raim_checkable = redundancy >= 1;
(
ObservabilityTier::Nominal,
raim_checkable,
raim_checkable || has_valid_prior,
)
};
GeometryQuality {
tier,
redundancy,
rank,
condition_number,
gdop,
raim_checkable,
covariance_validated,
}
}
fn exceeds_cutoff(value: f64, cutoff: f64) -> bool {
!value.is_finite() || !cutoff.is_finite() || value > cutoff
}
#[cfg(test)]
mod tests {
//! Clean-room tests derived from estimation-theory classification rules.
//! Redundancy, rank, condition number, GDOP, and prior availability are
//! explicit scalar inputs; expected tiers do not come from a solve.
use super::*;
fn thresholds() -> GeometryQualityThresholds {
GeometryQualityThresholds {
cond_cutoff: 100.0,
gdop_cutoff: 10.0,
}
}
#[test]
fn zero_redundancy_without_prior_is_not_validated() {
let quality = classify(4, 4, 0, 3.0, 2.0, false, thresholds());
assert_eq!(
quality,
GeometryQuality {
tier: ObservabilityTier::ZeroRedundancy,
redundancy: 0,
rank: 4,
condition_number: 3.0,
gdop: 2.0,
raim_checkable: false,
covariance_validated: false,
}
);
}
#[test]
fn zero_redundancy_with_prior_is_validated() {
let quality = classify(4, 4, 0, 3.0, 2.0, true, thresholds());
assert_eq!(quality.tier, ObservabilityTier::ZeroRedundancy);
assert!(!quality.raim_checkable);
assert!(quality.covariance_validated);
}
#[test]
fn rank_deficient_disables_raim_and_covariance_validation() {
let quality = classify(3, 4, 2, 2.0e12, 30.0, true, thresholds());
assert_eq!(quality.tier, ObservabilityTier::RankDeficient);
assert_eq!(quality.rank, 3);
assert_eq!(quality.redundancy, 2);
assert!(!quality.raim_checkable);
assert!(!quality.covariance_validated);
}
#[test]
fn weak_when_condition_number_exceeds_cutoff() {
let quality = classify(4, 4, 1, 100.0 + 1.0e-9, 2.0, false, thresholds());
assert_eq!(quality.tier, ObservabilityTier::Weak);
assert!(quality.raim_checkable);
assert!(quality.covariance_validated);
}
#[test]
fn weak_when_gdop_exceeds_cutoff() {
let quality = classify(4, 4, 1, 3.0, 10.0 + 1.0e-12, false, thresholds());
assert_eq!(quality.tier, ObservabilityTier::Weak);
assert!(quality.raim_checkable);
assert!(quality.covariance_validated);
}
#[test]
fn nominal_with_full_rank_and_positive_redundancy() {
let quality = classify(4, 4, 2, 20.0, 4.0, false, thresholds());
assert_eq!(
quality,
GeometryQuality {
tier: ObservabilityTier::Nominal,
redundancy: 2,
rank: 4,
condition_number: 20.0,
gdop: 4.0,
raim_checkable: true,
covariance_validated: true,
}
);
}
#[test]
fn condition_cutoff_boundary_is_strict() {
let at_cutoff = classify(4, 4, 1, 100.0, 2.0, false, thresholds());
let below_cutoff = classify(4, 4, 1, 100.0 - 1.0e-9, 2.0, false, thresholds());
let above_cutoff = classify(4, 4, 1, 100.0 + 1.0e-9, 2.0, false, thresholds());
assert_eq!(at_cutoff.tier, ObservabilityTier::Nominal);
assert_eq!(below_cutoff.tier, ObservabilityTier::Nominal);
assert_eq!(above_cutoff.tier, ObservabilityTier::Weak);
}
#[test]
fn gdop_cutoff_boundary_is_strict() {
let at_cutoff = classify(4, 4, 1, 3.0, 10.0, false, thresholds());
let below_cutoff = classify(4, 4, 1, 3.0, 10.0 - 1.0e-12, false, thresholds());
let above_cutoff = classify(4, 4, 1, 3.0, 10.0 + 1.0e-12, false, thresholds());
assert_eq!(at_cutoff.tier, ObservabilityTier::Nominal);
assert_eq!(below_cutoff.tier, ObservabilityTier::Nominal);
assert_eq!(above_cutoff.tier, ObservabilityTier::Weak);
}
}