Simulate double integrator with linear state feedback and constant cancelling feedforward.
This commit is contained in:
Hayden Heroux
parent
11562e223f
commit
fb40960e13
15 changed files with 466 additions and 8 deletions
235
Cargo.lock
generated
235
Cargo.lock
generated
|
|
@ -2,6 +2,241 @@
|
||||||
# It is not intended for manual editing.
|
# It is not intended for manual editing.
|
||||||
version = 4
|
version = 4
|
||||||
|
|
||||||
|
[[package]]
|
||||||
|
name = "approx"
|
||||||
|
version = "0.5.1"
|
||||||
|
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||||
|
checksum = "cab112f0a86d568ea0e627cc1d6be74a1e9cd55214684db5561995f6dad897c6"
|
||||||
|
dependencies = [
|
||||||
|
"num-traits",
|
||||||
|
]
|
||||||
|
|
||||||
|
[[package]]
|
||||||
|
name = "autocfg"
|
||||||
|
version = "1.5.1"
|
||||||
|
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||||
|
checksum = "f2032f911046de80f0a198e0901378627c33f59ea0ac00e363d481118bd70a53"
|
||||||
|
|
||||||
|
[[package]]
|
||||||
|
name = "bytemuck"
|
||||||
|
version = "1.25.2"
|
||||||
|
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||||
|
checksum = "95832e849adfb21180ccb6826a99da14e5d266ae5c2e668e1602cf234f153797"
|
||||||
|
|
||||||
|
[[package]]
|
||||||
|
name = "glam"
|
||||||
|
version = "0.30.10"
|
||||||
|
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||||
|
checksum = "19fc433e8437a212d1b6f1e68c7824af3aed907da60afa994e7f542d18d12aa9"
|
||||||
|
|
||||||
|
[[package]]
|
||||||
|
name = "glam"
|
||||||
|
version = "0.31.1"
|
||||||
|
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||||
|
checksum = "556f6b2ea90b8d15a74e0e7bb41671c9bdf38cd9f78c284d750b9ce58a2b5be7"
|
||||||
|
|
||||||
|
[[package]]
|
||||||
|
name = "glam"
|
||||||
|
version = "0.32.1"
|
||||||
|
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||||
|
checksum = "f70749695b063ecbf6b62949ccccde2e733ec3ecbbd71d467dca4e5c6c97cca0"
|
||||||
|
|
||||||
|
[[package]]
|
||||||
|
name = "glam"
|
||||||
|
version = "0.33.12"
|
||||||
|
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||||
|
checksum = "37bfe73dc8ec21f54d181e5a60554327ec3f1c600c455a75ccbfc65d2106a6c4"
|
||||||
|
|
||||||
[[package]]
|
[[package]]
|
||||||
name = "lyapunov"
|
name = "lyapunov"
|
||||||
version = "0.1.0"
|
version = "0.1.0"
|
||||||
|
dependencies = [
|
||||||
|
"nalgebra",
|
||||||
|
"uom",
|
||||||
|
]
|
||||||
|
|
||||||
|
[[package]]
|
||||||
|
name = "lyapunov-demo"
|
||||||
|
version = "0.1.0"
|
||||||
|
dependencies = [
|
||||||
|
"lyapunov",
|
||||||
|
"nalgebra",
|
||||||
|
"uom",
|
||||||
|
]
|
||||||
|
|
||||||
|
[[package]]
|
||||||
|
name = "matrixmultiply"
|
||||||
|
version = "0.3.11"
|
||||||
|
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||||
|
checksum = "3f607c237553f086e7043417a51df26b2eb899d3caff94e6a67592ff992fedc7"
|
||||||
|
dependencies = [
|
||||||
|
"autocfg",
|
||||||
|
"rawpointer",
|
||||||
|
]
|
||||||
|
|
||||||
|
[[package]]
|
||||||
|
name = "nalgebra"
|
||||||
|
version = "0.35.0"
|
||||||
|
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||||
|
checksum = "adc43a60c217b0c6ff46e47f26911015ad8d2e5a8be1af668c67e370d99a4346"
|
||||||
|
dependencies = [
|
||||||
|
"approx",
|
||||||
|
"glam 0.30.10",
|
||||||
|
"glam 0.31.1",
|
||||||
|
"glam 0.32.1",
|
||||||
|
"glam 0.33.12",
|
||||||
|
"matrixmultiply",
|
||||||
|
"nalgebra-macros",
|
||||||
|
"num-complex",
|
||||||
|
"num-rational",
|
||||||
|
"num-traits",
|
||||||
|
"simba",
|
||||||
|
"typenum",
|
||||||
|
]
|
||||||
|
|
||||||
|
[[package]]
|
||||||
|
name = "nalgebra-macros"
|
||||||
|
version = "0.3.0"
|
||||||
|
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||||
|
checksum = "973e7178a678cfd059ccec50887658d482ce16b0aa9da3888ddeab5cd5eb4889"
|
||||||
|
dependencies = [
|
||||||
|
"proc-macro2",
|
||||||
|
"quote",
|
||||||
|
"syn",
|
||||||
|
]
|
||||||
|
|
||||||
|
[[package]]
|
||||||
|
name = "num-bigint"
|
||||||
|
version = "0.4.8"
|
||||||
|
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||||
|
checksum = "c89e69e7e0f03bea5ef08013795c25018e101932225a656383bd384495ecc367"
|
||||||
|
dependencies = [
|
||||||
|
"num-integer",
|
||||||
|
"num-traits",
|
||||||
|
]
|
||||||
|
|
||||||
|
[[package]]
|
||||||
|
name = "num-complex"
|
||||||
|
version = "0.4.6"
|
||||||
|
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||||
|
checksum = "73f88a1307638156682bada9d7604135552957b7818057dcef22705b4d509495"
|
||||||
|
dependencies = [
|
||||||
|
"num-traits",
|
||||||
|
]
|
||||||
|
|
||||||
|
[[package]]
|
||||||
|
name = "num-integer"
|
||||||
|
version = "0.1.47"
|
||||||
|
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||||
|
checksum = "7ce2d95d4b3734dc35aa2f45e1aa22cd416814592a4f9d9205e11affd5b8e10b"
|
||||||
|
dependencies = [
|
||||||
|
"num-traits",
|
||||||
|
]
|
||||||
|
|
||||||
|
[[package]]
|
||||||
|
name = "num-rational"
|
||||||
|
version = "0.4.2"
|
||||||
|
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||||
|
checksum = "f83d14da390562dca69fc84082e73e548e1ad308d24accdedd2720017cb37824"
|
||||||
|
dependencies = [
|
||||||
|
"num-bigint",
|
||||||
|
"num-integer",
|
||||||
|
"num-traits",
|
||||||
|
]
|
||||||
|
|
||||||
|
[[package]]
|
||||||
|
name = "num-traits"
|
||||||
|
version = "0.2.19"
|
||||||
|
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||||
|
checksum = "071dfc062690e90b734c0b2273ce72ad0ffa95f0c74596bc250dcfd960262841"
|
||||||
|
dependencies = [
|
||||||
|
"autocfg",
|
||||||
|
]
|
||||||
|
|
||||||
|
[[package]]
|
||||||
|
name = "proc-macro2"
|
||||||
|
version = "1.0.107"
|
||||||
|
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||||
|
checksum = "985e7ec9bb745e6ce6535b544d84d6cd6f7ad8bd711c398938ae983b91a766d9"
|
||||||
|
dependencies = [
|
||||||
|
"unicode-ident",
|
||||||
|
]
|
||||||
|
|
||||||
|
[[package]]
|
||||||
|
name = "quote"
|
||||||
|
version = "1.0.47"
|
||||||
|
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||||
|
checksum = "1fbf4db142a473a8d80c26bbf18454ed458bf8d26c8219c331daecfdbd079001"
|
||||||
|
dependencies = [
|
||||||
|
"proc-macro2",
|
||||||
|
]
|
||||||
|
|
||||||
|
[[package]]
|
||||||
|
name = "rawpointer"
|
||||||
|
version = "0.2.1"
|
||||||
|
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||||
|
checksum = "60a357793950651c4ed0f3f52338f53b2f809f32d83a07f72909fa13e4c6c1e3"
|
||||||
|
|
||||||
|
[[package]]
|
||||||
|
name = "safe_arch"
|
||||||
|
version = "1.2.0"
|
||||||
|
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||||
|
checksum = "42c6efa15875e6ecb39ca61fb0b0c1a40b84fac5a5ffe71eef7d1000c8eb3f5f"
|
||||||
|
dependencies = [
|
||||||
|
"bytemuck",
|
||||||
|
]
|
||||||
|
|
||||||
|
[[package]]
|
||||||
|
name = "simba"
|
||||||
|
version = "0.10.2"
|
||||||
|
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||||
|
checksum = "f1a7200d82ff1c7b4235efd12f11e2537a402c91cf83a5cb97ce80eef787fde7"
|
||||||
|
dependencies = [
|
||||||
|
"approx",
|
||||||
|
"num-complex",
|
||||||
|
"num-traits",
|
||||||
|
"wide",
|
||||||
|
]
|
||||||
|
|
||||||
|
[[package]]
|
||||||
|
name = "syn"
|
||||||
|
version = "2.0.119"
|
||||||
|
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||||
|
checksum = "872831b642d1a07999a962a351ed35b955ea2cfc8f3862091e2a240a84f17297"
|
||||||
|
dependencies = [
|
||||||
|
"proc-macro2",
|
||||||
|
"quote",
|
||||||
|
"unicode-ident",
|
||||||
|
]
|
||||||
|
|
||||||
|
[[package]]
|
||||||
|
name = "typenum"
|
||||||
|
version = "1.20.1"
|
||||||
|
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||||
|
checksum = "b6f5e870be6c3b371b77fe0ee0bafb859fa4964b4404c27de1d380043c4dda20"
|
||||||
|
|
||||||
|
[[package]]
|
||||||
|
name = "unicode-ident"
|
||||||
|
version = "1.0.27"
|
||||||
|
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||||
|
checksum = "a2c754d6c33795a1c324727428e5a7dedb5b06195f9890bdbcba760d3e246563"
|
||||||
|
|
||||||
|
[[package]]
|
||||||
|
name = "uom"
|
||||||
|
version = "0.38.0"
|
||||||
|
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||||
|
checksum = "a739f83872836c82a4f2527d4e54b37007b3de68cafe7edde95fd695968bf4b9"
|
||||||
|
dependencies = [
|
||||||
|
"num-traits",
|
||||||
|
"typenum",
|
||||||
|
]
|
||||||
|
|
||||||
|
[[package]]
|
||||||
|
name = "wide"
|
||||||
|
version = "1.7.1"
|
||||||
|
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||||
|
checksum = "d920ac99c3c8edce110cb8d07dbb324d6d026011dce85b1e9355b70f0adacc4f"
|
||||||
|
dependencies = [
|
||||||
|
"bytemuck",
|
||||||
|
"safe_arch",
|
||||||
|
]
|
||||||
|
|
|
||||||
11
Cargo.toml
11
Cargo.toml
|
|
@ -1,6 +1,7 @@
|
||||||
[package]
|
[workspace]
|
||||||
name = "lyapunov"
|
resolver = "2"
|
||||||
version = "0.1.0"
|
members = ["crates/*"]
|
||||||
edition = "2024"
|
|
||||||
|
|
||||||
[dependencies]
|
[workspace.package]
|
||||||
|
edition = "2024"
|
||||||
|
version = "0.1.0"
|
||||||
|
|
|
||||||
9
crates/lyapunov-demo/Cargo.toml
Normal file
9
crates/lyapunov-demo/Cargo.toml
Normal file
|
|
@ -0,0 +1,9 @@
|
||||||
|
[package]
|
||||||
|
name = "lyapunov-demo"
|
||||||
|
version.workspace = true
|
||||||
|
edition.workspace = true
|
||||||
|
|
||||||
|
[dependencies]
|
||||||
|
lyapunov = { path = "../lyapunov" }
|
||||||
|
nalgebra = "0.35"
|
||||||
|
uom = "0.38"
|
||||||
30
crates/lyapunov-demo/src/main.rs
Normal file
30
crates/lyapunov-demo/src/main.rs
Normal file
|
|
@ -0,0 +1,30 @@
|
||||||
|
use lyapunov::{Controller, Integrator, LinearFeedback, LinearSystem, Rk4};
|
||||||
|
use nalgebra::SVector;
|
||||||
|
use uom::si::f64::Time;
|
||||||
|
use uom::si::time::millisecond;
|
||||||
|
|
||||||
|
fn main() {
|
||||||
|
let system = LinearSystem::<2, 1>::double_integrator(1.0, -0.1);
|
||||||
|
let u_ff = system
|
||||||
|
.constant_cancelling_input()
|
||||||
|
.unwrap_or_else(SVector::zeros);
|
||||||
|
|
||||||
|
let p = LinearFeedback::<2, 1>::from_gains([2.0, 3.0]);
|
||||||
|
|
||||||
|
let mut x = SVector::<f64, 2>::new(10.0, 0.0);
|
||||||
|
let r = SVector::<f64, 2>::new(0.0, 0.0);
|
||||||
|
let dt = Time::new::<millisecond>(50.0);
|
||||||
|
|
||||||
|
for step in 0..200 {
|
||||||
|
let t = dt * (step as f64);
|
||||||
|
|
||||||
|
let u_fb = p.calculate(&system, t, &x, &r);
|
||||||
|
let u = u_fb + u_ff;
|
||||||
|
x = Rk4.step(&system, t, &x, &u, dt);
|
||||||
|
|
||||||
|
println!(
|
||||||
|
"t={:.2}s u={:.3} pos={:.3}m vel={:.3}m/s",
|
||||||
|
t.value, u[0], x[0], x[1],
|
||||||
|
);
|
||||||
|
}
|
||||||
|
}
|
||||||
8
crates/lyapunov/Cargo.toml
Normal file
8
crates/lyapunov/Cargo.toml
Normal file
|
|
@ -0,0 +1,8 @@
|
||||||
|
[package]
|
||||||
|
name = "lyapunov"
|
||||||
|
version.workspace = true
|
||||||
|
edition.workspace = true
|
||||||
|
|
||||||
|
[dependencies]
|
||||||
|
uom = "0.38"
|
||||||
|
nalgebra = "0.35"
|
||||||
39
crates/lyapunov/src/controllers/linear_feedback.rs
Normal file
39
crates/lyapunov/src/controllers/linear_feedback.rs
Normal file
|
|
@ -0,0 +1,39 @@
|
||||||
|
use nalgebra::{SMatrix, SVector};
|
||||||
|
use uom::si::f64::Time;
|
||||||
|
|
||||||
|
use crate::traits::{Controller, System};
|
||||||
|
|
||||||
|
pub struct LinearFeedback<const N: usize, const M: usize> {
|
||||||
|
pub gain_matrix: SMatrix<f64, M, N>,
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<S, const N: usize, const M: usize> Controller<S, N, M> for LinearFeedback<N, M>
|
||||||
|
where
|
||||||
|
S: System<N, M, State = SVector<f64, N>, Input = SVector<f64, M>, Derivative = SVector<f64, N>>,
|
||||||
|
S::State: Copy + std::ops::Sub<Output = S::State>,
|
||||||
|
{
|
||||||
|
fn calculate(&self, _system: &S, _t: Time, x: &S::State, r: &S::State) -> S::Input {
|
||||||
|
let error = *r - *x;
|
||||||
|
self.gain_matrix * error
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<const N: usize> LinearFeedback<N, 1> {
|
||||||
|
pub fn from_gains(gains: [f64; N]) -> Self {
|
||||||
|
let mut gain_matrix = SMatrix::<f64, 1, N>::zeros();
|
||||||
|
for (j, &g) in gains.iter().enumerate() {
|
||||||
|
gain_matrix[(0, j)] = g;
|
||||||
|
}
|
||||||
|
Self { gain_matrix }
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<const N: usize> LinearFeedback<N, N> {
|
||||||
|
pub fn diagonal(gains: [f64; N]) -> Self {
|
||||||
|
let mut gain_matrix = SMatrix::<f64, N, N>::zeros();
|
||||||
|
for i in 0..N {
|
||||||
|
gain_matrix[(i, i)] = gains[i];
|
||||||
|
}
|
||||||
|
Self { gain_matrix }
|
||||||
|
}
|
||||||
|
}
|
||||||
1
crates/lyapunov/src/controllers/mod.rs
Normal file
1
crates/lyapunov/src/controllers/mod.rs
Normal file
|
|
@ -0,0 +1 @@
|
||||||
|
pub mod linear_feedback;
|
||||||
5
crates/lyapunov/src/integrators/mod.rs
Normal file
5
crates/lyapunov/src/integrators/mod.rs
Normal file
|
|
@ -0,0 +1,5 @@
|
||||||
|
pub mod rk4;
|
||||||
|
pub mod traits;
|
||||||
|
|
||||||
|
pub use rk4::Rk4;
|
||||||
|
pub use traits::Accumulate;
|
||||||
34
crates/lyapunov/src/integrators/rk4.rs
Normal file
34
crates/lyapunov/src/integrators/rk4.rs
Normal file
|
|
@ -0,0 +1,34 @@
|
||||||
|
use uom::si::f64::Time;
|
||||||
|
|
||||||
|
use crate::{
|
||||||
|
integrators::traits::Accumulate,
|
||||||
|
traits::{Integrator, System},
|
||||||
|
};
|
||||||
|
|
||||||
|
pub struct Rk4;
|
||||||
|
|
||||||
|
impl<S, const N: usize, const M: usize> Integrator<S, N, M> for Rk4
|
||||||
|
where
|
||||||
|
S: System<N, M>,
|
||||||
|
S::State: Clone + Accumulate<S::Derivative>,
|
||||||
|
{
|
||||||
|
fn step(&self, system: &S, t: Time, x: &S::State, u: &S::Input, dt: Time) -> S::State {
|
||||||
|
let half_dt = dt * 0.5;
|
||||||
|
|
||||||
|
let k1 = system.derivatives(t, x, u);
|
||||||
|
|
||||||
|
let x2 = x.add_scaled(half_dt, &k1);
|
||||||
|
let k2 = system.derivatives(t + half_dt, &x2, u);
|
||||||
|
|
||||||
|
let x3 = x.add_scaled(half_dt, &k2);
|
||||||
|
let k3 = system.derivatives(t + half_dt, &x3, u);
|
||||||
|
|
||||||
|
let x4 = x.add_scaled(dt, &k3);
|
||||||
|
let k4 = system.derivatives(t + dt, &x4, u);
|
||||||
|
|
||||||
|
x.add_scaled(dt / 6.0, &k1)
|
||||||
|
.add_scaled(dt / 3.0, &k2)
|
||||||
|
.add_scaled(dt / 3.0, &k3)
|
||||||
|
.add_scaled(dt / 6.0, &k4)
|
||||||
|
}
|
||||||
|
}
|
||||||
12
crates/lyapunov/src/integrators/traits.rs
Normal file
12
crates/lyapunov/src/integrators/traits.rs
Normal file
|
|
@ -0,0 +1,12 @@
|
||||||
|
use nalgebra::SVector;
|
||||||
|
use uom::si::f64::Time;
|
||||||
|
|
||||||
|
pub trait Accumulate<D> {
|
||||||
|
fn add_scaled(&self, dt: Time, derivative: &D) -> Self;
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<const N: usize> Accumulate<SVector<f64, N>> for SVector<f64, N> {
|
||||||
|
fn add_scaled(&self, dt: Time, derivative: &SVector<f64, N>) -> Self {
|
||||||
|
self + dt.get::<uom::si::time::second>() * derivative
|
||||||
|
}
|
||||||
|
}
|
||||||
9
crates/lyapunov/src/lib.rs
Normal file
9
crates/lyapunov/src/lib.rs
Normal file
|
|
@ -0,0 +1,9 @@
|
||||||
|
pub mod controllers;
|
||||||
|
pub mod integrators;
|
||||||
|
pub mod systems;
|
||||||
|
pub mod traits;
|
||||||
|
|
||||||
|
pub use controllers::linear_feedback::LinearFeedback;
|
||||||
|
pub use integrators::rk4::Rk4;
|
||||||
|
pub use systems::linear_system::LinearSystem;
|
||||||
|
pub use traits::{Controller, Integrator, System};
|
||||||
47
crates/lyapunov/src/systems/linear_system.rs
Normal file
47
crates/lyapunov/src/systems/linear_system.rs
Normal file
|
|
@ -0,0 +1,47 @@
|
||||||
|
use crate::traits::System;
|
||||||
|
use nalgebra::{DMatrix, DVector, SMatrix, SVector};
|
||||||
|
use uom::si::f64::Time;
|
||||||
|
|
||||||
|
pub struct LinearSystem<const N: usize, const M: usize> {
|
||||||
|
pub state_matrix: SMatrix<f64, N, N>,
|
||||||
|
pub input_matrix: SMatrix<f64, N, M>,
|
||||||
|
pub constant: SVector<f64, N>,
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<const N: usize, const M: usize> LinearSystem<N, M> {
|
||||||
|
pub fn constant_cancelling_input(&self) -> Option<SVector<f64, M>> {
|
||||||
|
let constant_cancel = -self.constant;
|
||||||
|
|
||||||
|
let input_matrix_dyn = DMatrix::from_column_slice(N, M, self.input_matrix.as_slice());
|
||||||
|
let constant_cancel_dyn = DVector::from_column_slice(constant_cancel.as_slice());
|
||||||
|
|
||||||
|
let svd = input_matrix_dyn.svd(true, true);
|
||||||
|
let input_dyn = svd.solve(&constant_cancel_dyn, 1e-10).ok()?;
|
||||||
|
|
||||||
|
let mut input = SVector::<f64, M>::zeros();
|
||||||
|
for i in 0..M {
|
||||||
|
input[i] = input_dyn[i];
|
||||||
|
}
|
||||||
|
Some(input)
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<const N: usize, const M: usize> System<N, M> for LinearSystem<N, M> {
|
||||||
|
type State = SVector<f64, N>;
|
||||||
|
type Input = SVector<f64, M>;
|
||||||
|
type Derivative = SVector<f64, N>;
|
||||||
|
|
||||||
|
fn derivatives(&self, _t: Time, x: &Self::State, u: &Self::Input) -> Self::Derivative {
|
||||||
|
self.state_matrix * x + self.input_matrix * u + self.constant
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl LinearSystem<2, 1> {
|
||||||
|
pub fn double_integrator(input_gain: f64, disturbance: f64) -> Self {
|
||||||
|
Self {
|
||||||
|
state_matrix: SMatrix::<f64, 2, 2>::from_row_slice(&[0.0, 1.0, 0.0, 0.0]),
|
||||||
|
input_matrix: SMatrix::<f64, 2, 1>::from_column_slice(&[0.0, input_gain]),
|
||||||
|
constant: SVector::<f64, 2>::from_column_slice(&[0.0, -disturbance]),
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
3
crates/lyapunov/src/systems/mod.rs
Normal file
3
crates/lyapunov/src/systems/mod.rs
Normal file
|
|
@ -0,0 +1,3 @@
|
||||||
|
pub mod linear_system;
|
||||||
|
|
||||||
|
pub use linear_system::LinearSystem;
|
||||||
28
crates/lyapunov/src/traits.rs
Normal file
28
crates/lyapunov/src/traits.rs
Normal file
|
|
@ -0,0 +1,28 @@
|
||||||
|
use uom::si::f64::*;
|
||||||
|
|
||||||
|
// Represents a dynamical system with state dimension N and input dimension M.
|
||||||
|
pub trait System<const N: usize, const M: usize> {
|
||||||
|
type State;
|
||||||
|
type Input;
|
||||||
|
type Derivative;
|
||||||
|
|
||||||
|
// Evaluate the derivative vector x_dot at (t, x, u)
|
||||||
|
// TODO Create time-invariant system trait?
|
||||||
|
fn derivatives(&self, t: Time, x: &Self::State, u: &Self::Input) -> Self::Derivative;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Computes the evolution of a system over (t, x, u)
|
||||||
|
pub trait Integrator<S, const N: usize, const M: usize>
|
||||||
|
where
|
||||||
|
S: System<N, M>,
|
||||||
|
{
|
||||||
|
// Performs a single simulation step of size dt for the system
|
||||||
|
fn step(&self, system: &S, t: Time, x: &S::State, u: &S::Input, dt: Time) -> S::State;
|
||||||
|
}
|
||||||
|
|
||||||
|
pub trait Controller<S, const N: usize, const M: usize>
|
||||||
|
where
|
||||||
|
S: System<N, M>,
|
||||||
|
{
|
||||||
|
fn calculate(&self, system: &S, t: Time, x: &S::State, r: &S::State) -> S::Input;
|
||||||
|
}
|
||||||
|
|
@ -1,3 +0,0 @@
|
||||||
fn main() {
|
|
||||||
println!("Hello, world!");
|
|
||||||
}
|
|
||||||
Loading…
Add table
Reference in a new issue