Skip to main content

flight_solver/cls/setup/
ls.rs

1//! Unregularised least-squares problem formulation.
2//!
3//! Builds `A = Wv · G` and `b = Wv · v` without the actuator-preference
4//! regularisation term. The coefficient matrix is `NV × NU` (no extra rows).
5//! Use with [`solve_cls`](crate::cls::solve_cls).
6
7use nalgebra::{allocator::Allocator, Const, DefaultAllocator, DimMin, DimName};
8
9use crate::cls::types::{Mat, VecN};
10
11/// Build the unregularised coefficient matrix `A = Wv · G`.
12///
13/// Returns an `NV × NU` matrix suitable for [`solve_cls`](crate::cls::solve_cls).
14pub fn setup_a<const NU: usize, const NV: usize>(b_mat: &Mat<NV, NU>, wv: &VecN<NV>) -> Mat<NV, NU>
15where
16    Const<NV>: DimName + DimMin<Const<NU>, Output = Const<NU>>,
17    Const<NU>: DimName,
18    DefaultAllocator: Allocator<Const<NV>, Const<NU>>
19        + Allocator<Const<NV>, Const<NV>>
20        + Allocator<Const<NU>, Const<NU>>
21        + Allocator<Const<NV>>
22        + Allocator<Const<NU>>,
23{
24    let mut a: Mat<NV, NU> = Mat::zeros();
25    for j in 0..NU {
26        for i in 0..NV {
27            a[(i, j)] = wv[i] * b_mat[(i, j)];
28        }
29    }
30    a
31}
32
33/// Build the unregularised right-hand side `b = Wv · v`.
34pub fn setup_b<const NV: usize>(v: &VecN<NV>, wv: &VecN<NV>) -> VecN<NV>
35where
36    Const<NV>: DimName,
37    DefaultAllocator: Allocator<Const<NV>>,
38{
39    let mut b: VecN<NV> = VecN::zeros();
40    for i in 0..NV {
41        b[i] = wv[i] * v[i];
42    }
43    b
44}