flight_solver/cls/setup/
wls.rs

1//! Weighted least-squares (WLS) problem formulation.
2//!
3//! Converts control-allocation parameters (effectiveness matrix `G`,
4//! pseudo-control weights `Wv`, actuator weights `Wu`) into the regularised
5//! `min ‖Au − b‖²` form that [`solve`](crate::cls::solve) accepts.
6//! The coefficient matrix `A` is `(NV + NU) × NU`.
7
8use nalgebra::{allocator::Allocator, Const, DefaultAllocator, DimMin, DimName};
9
10use crate::cls::types::{Mat, VecN};
11
12/// Minimum diagonal value used when normalising actuator weights.
13const MIN_DIAG_CLAMP: f32 = 1e-6;
14
15#[allow(clippy::needless_range_loop)]
16fn gamma_estimator<const NV: usize>(a2: &[[f32; NV]; NV], cond_target: f32) -> (f32, f32) {
17    let mut max_sig: f32 = 0.0;
18    for i in 0..NV {
19        let mut r: f32 = 0.0;
20        for j in 0..NV {
21            if j != i {
22                r += libm::fabsf(a2[i][j]);
23            }
24        }
25        let disk = a2[i][i] + r;
26        if max_sig < disk {
27            max_sig = disk;
28        }
29    }
30    (libm::sqrtf(max_sig / cond_target), max_sig)
31}
32
33/// Convert WLS control allocation to a least-squares problem `min ‖Au − b‖²`.
34///
35/// `wu` is **normalized in-place** by its minimum value (matching the C code).
36/// Returns `(A, gamma)`.
37#[allow(clippy::needless_range_loop)]
38pub fn setup_a<const NU: usize, const NV: usize, const NC: usize>(
39    b_mat: &Mat<NV, NU>,
40    wv: &VecN<NV>,
41    wu: &mut VecN<NU>,
42    theta: f32,
43    cond_bound: f32,
44) -> (Mat<NC, NU>, f32)
45where
46    Const<NC>: DimName + DimMin<Const<NU>, Output = Const<NU>>,
47    Const<NU>: DimName,
48    Const<NV>: DimName,
49    DefaultAllocator: Allocator<Const<NC>, Const<NU>>
50        + Allocator<Const<NC>, Const<NC>>
51        + Allocator<Const<NU>, Const<NU>>
52        + Allocator<Const<NC>>
53        + Allocator<Const<NU>>
54        + Allocator<Const<NV>>,
55{
56    debug_assert_eq!(NC, NU + NV);
57
58    let mut a2 = [[0.0f32; NV]; NV];
59    for i in 0..NV {
60        for j in i..NV {
61            let mut sum = 0.0f32;
62            for k in 0..NU {
63                sum += b_mat[(i, k)] * b_mat[(j, k)];
64            }
65            a2[i][j] = sum * wv[i] * wv[i];
66            if i != j {
67                a2[j][i] = a2[i][j];
68            }
69        }
70    }
71
72    let mut min_diag: f32 = f32::INFINITY;
73    let mut max_diag: f32 = 0.0;
74    for i in 0..NU {
75        if wu[i] < min_diag {
76            min_diag = wu[i];
77        }
78        if wu[i] > max_diag {
79            max_diag = wu[i];
80        }
81    }
82    if min_diag < MIN_DIAG_CLAMP {
83        min_diag = MIN_DIAG_CLAMP;
84    }
85    let inv = 1.0 / min_diag;
86    for i in 0..NU {
87        wu[i] *= inv;
88    }
89    max_diag *= inv;
90
91    let gamma = if cond_bound > 0.0 {
92        let (ge, ms) = gamma_estimator(&a2, cond_bound);
93        let gt = libm::sqrtf(ms) * theta / max_diag;
94        if ge > gt {
95            ge
96        } else {
97            gt
98        }
99    } else {
100        let (_, ms) = gamma_estimator(&a2, 1.0);
101        libm::sqrtf(ms) * theta / max_diag
102    };
103
104    let mut a: Mat<NC, NU> = Mat::zeros();
105    for j in 0..NU {
106        for i in 0..NV {
107            a[(i, j)] = wv[i] * b_mat[(i, j)];
108        }
109        a[(NV + j, j)] = gamma * wu[j];
110    }
111
112    (a, gamma)
113}
114
115/// Compute the right-hand side `b` for the regularised WLS problem.
116pub fn setup_b<const NU: usize, const NV: usize, const NC: usize>(
117    v: &VecN<NV>,
118    ud: &VecN<NU>,
119    wv: &VecN<NV>,
120    wu_norm: &VecN<NU>,
121    gamma: f32,
122) -> VecN<NC>
123where
124    Const<NC>: DimName + DimMin<Const<NU>, Output = Const<NU>>,
125    Const<NU>: DimName,
126    Const<NV>: DimName,
127    DefaultAllocator: Allocator<Const<NC>, Const<NU>>
128        + Allocator<Const<NC>, Const<NC>>
129        + Allocator<Const<NU>, Const<NU>>
130        + Allocator<Const<NC>>
131        + Allocator<Const<NU>>
132        + Allocator<Const<NV>>,
133{
134    debug_assert_eq!(NC, NU + NV);
135    let mut b: VecN<NC> = VecN::zeros();
136    for i in 0..NV {
137        b[i] = wv[i] * v[i];
138    }
139    for i in 0..NU {
140        b[NV + i] = gamma * wu_norm[i] * ud[i];
141    }
142    b
143}
flight_solver/cls/setup/wls.rs

flight_solver/cls/setup/
wls.rs
