pub fn std_f32(src: &[f32], result: &mut f32) -> StatusExamples found in repository?
examples/sensor_fusion_navigation.rs (line 279)
64fn main() {
65 println!("===============================================================================");
66 println!(" embedded-dsp Sensor Fusion, Navigation & Attitude Estimation ");
67 println!("===============================================================================");
68 println!();
69
70 // -----------------------------------------------------------------------------------------
71 // 1. IMU Sensor Outlier Rejection via 1D Conditional Median Filtering
72 // -----------------------------------------------------------------------------------------
73 println!("--- 1. Sensor Conditioning & Glitch Removal (Conditional Median Filter) ---");
74 // Accelerometer readings experiencing occasional mechanical shock / communication glitches
75 let raw_accel_z = [
76 9.81f32, 9.80, 9.82, 105.4, 9.81, 9.79, 9.83, 9.81, -45.0, 9.82, 9.80, 9.81,
77 ];
78 let mut cleaned_accel_z = [0.0f32; 12];
79 // Replaces spike only if it deviates by more than threshold (10.0 m/s^2) from local median
80 median_filter_1d_f32(&raw_accel_z, &mut cleaned_accel_z, 3, 10.0);
81
82 println!(" Raw Accel Z (with spikes) : {:?}", raw_accel_z);
83 println!(" Cleaned Accel Z (spikes fixed): {:?}", cleaned_accel_z);
84
85 // -----------------------------------------------------------------------------------------
86 // 2. Sensor Factory Calibration via Weighted Polynomial Least Squares
87 // -----------------------------------------------------------------------------------------
88 println!("\n--- 2. Sensor Factory Calibration (Polynomial Least-Squares Fit) ---");
89 // Calibration fixture measurements: ADC readings vs Known physical quantities (e.g. pressure/temp)
90 let adc_counts = [100.0f32, 200.0, 300.0, 400.0, 500.0];
91 // True relationship: Output = 12.5 + 0.45 * ADC
92 let ref_values = [57.5f32, 102.5, 147.5, 192.5, 237.5];
93 let mut calib_params = [0.0f32; 2]; // [offset c0, gain c1]
94
95 let status = polynomial_least_squares_fit(&adc_counts, &ref_values, None, 1, &mut calib_params);
96 if status == Status::Success {
97 println!(
98 " Fitted Calibration Model: y = {:.4} + {:.4} * x",
99 calib_params[0], calib_params[1]
100 );
101 } else {
102 println!(" Least squares fitting error: {:?}", status);
103 }
104
105 // -----------------------------------------------------------------------------------------
106 // 3. 3D Orientation & Attitude Tracking with Quaternions
107 // -----------------------------------------------------------------------------------------
108 println!("\n--- 3. 3D Attitude Estimation with Unit Quaternions ---");
109 // Initial attitude: Identity (no rotation)
110 let q_current = [1.0f32, 0.0, 0.0, 0.0]; // [w, x, y, z]
111
112 // Incremental rotation: Pitch 90 degrees around Y-axis (cos(45°), 0, sin(45°), 0)
113 let angle_y = core::f32::consts::FRAC_PI_2;
114 let q_pitch_90 = [(angle_y / 2.0).cos(), 0.0, (angle_y / 2.0).sin(), 0.0];
115
116 let mut q_rotated = [0.0f32; 4];
117 quaternion_product_f32(&q_pitch_90, &q_current, &mut q_rotated);
118 quaternion_normalize_f32(&mut q_rotated);
119 println!(
120 " Quaternion after 90° Pitch: [{:.4}, {:.4}, {:.4}, {:.4}]",
121 q_rotated[0], q_rotated[1], q_rotated[2], q_rotated[3]
122 );
123
124 // Convert attitude to 3x3 Rotation Matrix
125 let mut rot_matrix = [0.0f32; 9];
126 quaternion_to_rotmat_f32(&q_rotated, &mut rot_matrix);
127 println!(" Converted 3x3 Direction Cosine Matrix (DCM):");
128 println!(
129 " [{:>7.4}, {:>7.4}, {:>7.4}]",
130 rot_matrix[0], rot_matrix[1], rot_matrix[2]
131 );
132 println!(
133 " [{:>7.4}, {:>7.4}, {:>7.4}]",
134 rot_matrix[3], rot_matrix[4], rot_matrix[5]
135 );
136 println!(
137 " [{:>7.4}, {:>7.4}, {:>7.4}]",
138 rot_matrix[6], rot_matrix[7], rot_matrix[8]
139 );
140
141 // Rotate body-frame vector (e.g. forward velocity [1.0, 0.0, 0.0]) to navigation-frame
142 // v_rot = q * v * q^*
143 let mut q_conj = [0.0f32; 4];
144 quaternion_conjugate_f32(&q_rotated, &mut q_conj);
145 let v_body = [0.0f32, 1.0, 0.0, 0.0]; // pure imaginary quaternion [0, vx, vy, vz]
146 let mut q_temp = [0.0f32; 4];
147 let mut v_nav_q = [0.0f32; 4];
148 quaternion_product_f32(&q_rotated, &v_body, &mut q_temp);
149 quaternion_product_f32(&q_temp, &q_conj, &mut v_nav_q);
150 println!(
151 " Body Vector [1, 0, 0] rotated to Navigation Frame: [{:.4}, {:.4}, {:.4}]",
152 v_nav_q[1], v_nav_q[2], v_nav_q[3]
153 );
154
155 // -----------------------------------------------------------------------------------------
156 // 4. Linear 2D Kinematic Kalman Filter (GPS Position + Velocity Fusion)
157 // -----------------------------------------------------------------------------------------
158 println!("\n--- 4. 2D Kinematic Kalman Filter (Position + Velocity Fusion) ---");
159 // True trajectory: moving at constant speed 2.0 m/s from pos = 0
160 const DT: f32 = 0.1; // 100 ms time step
161 const STEPS: usize = 30;
162
163 let mut kf2d = KalmanFilter2D::new(0.0, 0.0, 0.1, 4.0); // q_var = 0.1, r_var = 4.0 (noisy GPS)
164 let mut estimated_pos = [0.0f32; STEPS];
165 let mut true_pos = [0.0f32; STEPS];
166
167 for k in 0..STEPS {
168 let t = k as f32 * DT;
169 let true_p = 2.0 * t;
170 true_pos[k] = true_p;
171
172 // Noisy GPS reading: True pos + random noise
173 let prng =
174 ((k as u64).wrapping_mul(1664525).wrapping_add(1013904223) % 1000) as f32 / 1000.0;
175 let noisy_gps = true_p + (prng - 0.5) * 3.0;
176
177 kf2d.predict(DT);
178 let est = kf2d.update(noisy_gps);
179 estimated_pos[k] = est[0];
180
181 if k % 10 == 0 || k == STEPS - 1 {
182 println!(
183 " Step {:>2} (t={:.1}s): True Pos={:>5.2}m, Noisy GPS={:>5.2}m, KF Pos={:>5.2}m, KF Vel={:>5.2}m/s",
184 k, t, true_p, noisy_gps, est[0], est[1]
185 );
186 }
187 }
188
189 // -----------------------------------------------------------------------------------------
190 // 5. Const-Generic Linear Kalman Filter (4-State, 2-Measurement)
191 // -----------------------------------------------------------------------------------------
192 println!("\n--- 5. Const-Generic Linear Kalman Filter (4x2 Tracking) ---");
193 // State: [px, vx, py, vy]
194 let f_matrix = [
195 [1.0, DT, 0.0, 0.0],
196 [0.0, 1.0, 0.0, 0.0],
197 [0.0, 0.0, 1.0, DT],
198 [0.0, 0.0, 0.0, 1.0],
199 ];
200 // Measurement matrix: GPS measures [px, py]
201 let h_matrix = [[1.0, 0.0, 0.0, 0.0], [0.0, 0.0, 1.0, 0.0]];
202 let mut p_cov = [[0.0f32; 4]; 4];
203 let mut q_cov = [[0.0f32; 4]; 4];
204 for i in 0..4 {
205 p_cov[i][i] = 1.0;
206 q_cov[i][i] = 0.05;
207 }
208 let r_cov = [[1.5, 0.0], [0.0, 1.5]];
209
210 let mut kf_4x2 = KalmanFilter::<4, 2>::new(
211 [0.0, 1.5, 0.0, -1.0], // Initial state
212 p_cov,
213 q_cov,
214 r_cov,
215 );
216
217 kf_4x2.predict(&f_matrix);
218 let meas_z = [0.18, -0.09];
219 let kf_status = kf_4x2.update(&h_matrix, &meas_z);
220 println!(
221 " Const-generic KalmanFilter<4, 2> update status: {:?}",
222 kf_status
223 );
224 println!(
225 " Updated State Vector: px={:.3}m, vx={:.3}m/s, py={:.3}m, vy={:.3}m/s",
226 kf_4x2.x[0], kf_4x2.x[1], kf_4x2.x[2], kf_4x2.x[3]
227 );
228
229 // -----------------------------------------------------------------------------------------
230 // 6. Non-Linear Extended Kalman Filter (Radar Range + Bearing Tracking)
231 // -----------------------------------------------------------------------------------------
232 println!("\n--- 6. Non-Linear Extended Kalman Filter (Radar Tracking) ---");
233 let ekf_model = RadarTrackingModel;
234 let mut ekf = ExtendedKalmanFilter::<4, 2, RadarTrackingModel>::from_variances(
235 [100.0, 10.0, 50.0, 5.0], // Target starts at (100m, 50m), speed (10m/s, 5m/s)
236 10.0, // Initial P variance
237 0.2, // Process Q variance
238 1.0, // Measurement R variance
239 ekf_model,
240 );
241
242 println!(" Target True Trajectory vs EKF Non-Linear Estimate:");
243 for step in 1..=5 {
244 let dt = 0.5;
245 // True target state
246 let true_px = 100.0 + step as f32 * dt * 10.0;
247 let true_py = 50.0 + step as f32 * dt * 5.0;
248
249 // Polar radar measurements with sensor noise
250 let true_range = (true_px * true_px + true_py * true_py).sqrt();
251 let true_bearing = true_py.atan2(true_px);
252
253 ekf.predict(dt);
254 let z = [true_range + 0.5, true_bearing - 0.005]; // Noisy radar ping
255 let status = ekf.update(&z);
256
257 println!(
258 " Step {}: Meas [Range={:>6.1}m, Azimuth={:>6.3} rad] -> EKF Est [X={:>6.1}m, Y={:>6.1}m] (Status: {:?})",
259 step, z[0], z[1], ekf.x[0], ekf.x[2], status
260 );
261 }
262
263 // -----------------------------------------------------------------------------------------
264 // 7. Statistical Performance Evaluation
265 // -----------------------------------------------------------------------------------------
266 println!("\n--- 7. Tracking Error Statistical Metrics ---");
267 let mut tracking_errors = [0.0f32; STEPS];
268 for i in 0..STEPS {
269 tracking_errors[i] = estimated_pos[i] - true_pos[i];
270 }
271
272 let mut mean_err = 0.0f32;
273 let mut std_err = 0.0f32;
274 let mut rms_err = 0.0f32;
275 let mut max_err = 0.0f32;
276 let mut max_idx = 0usize;
277
278 mean_f32(&tracking_errors, &mut mean_err);
279 std_f32(&tracking_errors, &mut std_err);
280 rms_f32(&tracking_errors, &mut rms_err);
281 max_f32(&tracking_errors, &mut max_err, &mut max_idx);
282
283 println!(" Position Tracking Error Metrics (over {} steps):", STEPS);
284 println!(" • Mean Error : {:>7.4} m", mean_err);
285 println!(" • Std Deviation : {:>7.4} m", std_err);
286 println!(" • RMS Error : {:>7.4} m", rms_err);
287 println!(
288 " • Max Error : {:>7.4} m (at step {})",
289 max_err, max_idx
290 );
291
292 println!();
293 println!("===============================================================================");
294 println!(" Sensor Fusion & Navigation Execution Complete! ");
295 println!("===============================================================================");
296}