kfilter/
system.rs

1//! A system performs the modelling role in the Kalman filter, predicting the next state
2//! based on the current state and any inputs. A system must implement the [System] trait
3//! and either [InputSystem] or [NoInputSystem] accordingly.
4//!
5//! Typically one of
6//! [LinearSystem], [LinearNoInputSystem] or [NonLinearSystem] will be used and these will
7//! be created automatically in one of the [Kalman](crate::kalman::Kalman) or
8//! [Kalman1M](crate::kalman::Kalman1M) constructors.
9
10use nalgebra::{RealField, SMatrix, SVector};
11
12// ================================== Traits =================================
13
14/// Base trait for a system which must also implement [InputSystem] or [NoInputSystem].
15pub trait System<T, const N: usize, const U: usize> {
16    /// Get a reference to the process covariance matrix
17    fn covariance(&self) -> &SMatrix<T, N, N>;
18    /// Get a reference to the state
19    fn state(&self) -> &SVector<T, N>;
20    /// Get a mutable reference to the state
21    fn state_mut(&mut self) -> &mut SVector<T, N>;
22}
23
24/// A System which has (or can generate) a transition matrix/Jacobian
25pub trait LinearisableSystem<T, const N: usize, const U: usize>: System<T, N, U> {
26    /// Get the transition matrix (Jacobian)
27    fn transition(&self) -> &SMatrix<T, N, N>;
28    /// Get the transpose of the transition matrix
29    fn transition_transpose(&self) -> &SMatrix<T, N, N>;
30}
31
32/// A System with an input.
33pub trait InputSystem<T, const N: usize, const U: usize>: System<T, N, U> {
34    /// transition to the next state, returning a reference to it
35    fn step(&mut self, u: SVector<T, U>) -> &SVector<T, N> {
36        *self.state_mut() = self.predict(self.state(), &u);
37        self.state()
38    }
39    /// Predict the next state based on the given state and input
40    fn predict(&self, x: &SVector<T, N>, u: &SVector<T, U>) -> SVector<T, N>;
41}
42
43// ========================== Linear Systems =================================
44/// A System without an input.
45pub trait NoInputSystem<T, const N: usize>: System<T, N, 0> {
46    /// transition to the next state, returning a reference to it
47    fn step(&mut self) -> &SVector<T, N> {
48        *self.state_mut() = self.predict(self.state());
49        self.state()
50    }
51    /// Predict the next state based on the given state
52    fn predict(&self, x: &SVector<T, N>) -> SVector<T, N>;
53}
54
55/// A linear system with an input.
56/// Defined by the transition matrix F, control matrix B and covariance matrix Q.
57#[allow(non_snake_case)]
58#[derive(Debug, Clone)]
59#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
60#[cfg_attr(feature = "defmt", derive(defmt::Format))]
61// #[cfg_attr(feature = "defmt", defmt(bound(T: ::defmt::Format, [T;N]: ::defmt::Format)))]
62pub struct LinearSystem<T: RealField, const N: usize, const U: usize> {
63    x: SVector<T, N>,
64    F: SMatrix<T, N, N>,
65    F_t: SMatrix<T, N, N>,
66    Q: SMatrix<T, N, N>,
67    B: SMatrix<T, N, U>,
68}
69
70#[allow(non_snake_case)]
71impl<T: RealField + Copy, const N: usize, const U: usize> LinearSystem<T, N, U> {
72    /// Create new [LinearSystem] from the transition matrix F, process covariance Q
73    /// and control matrix B.
74    pub fn new(
75        F: SMatrix<T, N, N>,
76        Q: SMatrix<T, N, N>,
77        B: SMatrix<T, N, U>,
78        x_initial: SVector<T, N>,
79    ) -> Self {
80        LinearSystem {
81            x: x_initial,
82            F,
83            F_t: F.transpose(),
84            Q,
85            B,
86        }
87    }
88    /// Set a new transition matrix, also updating the transpose
89    pub fn set_transition(&mut self, transition: SMatrix<T, N, N>) {
90        self.F_t = transition.transpose();
91        self.F = transition;
92    }
93    /// Get a mutable reference to the process covariance matrix
94    pub fn covariance_mut(&mut self) -> &mut SMatrix<T, N, N> {
95        &mut self.Q
96    }
97}
98
99/// Implement [System] for [LinearSystem]
100impl<T: RealField + Copy, const N: usize, const U: usize> System<T, N, U>
101    for LinearSystem<T, N, U>
102{
103    fn covariance(&self) -> &SMatrix<T, N, N> {
104        &self.Q
105    }
106
107    fn state(&self) -> &SVector<T, N> {
108        &self.x
109    }
110
111    fn state_mut(&mut self) -> &mut SVector<T, N> {
112        &mut self.x
113    }
114}
115
116impl<T: RealField + Copy, const N: usize, const U: usize> LinearisableSystem<T, N, U>
117    for LinearSystem<T, N, U>
118{
119    fn transition(&self) -> &SMatrix<T, N, N> {
120        &self.F
121    }
122    fn transition_transpose(&self) -> &SMatrix<T, N, N> {
123        &self.F_t
124    }
125}
126
127/// impl [InputSystem] for [LinearSystem]
128impl<T: RealField + Copy, const N: usize, const U: usize> InputSystem<T, N, U>
129    for LinearSystem<T, N, U>
130{
131    fn predict(&self, x: &SVector<T, N>, u: &SVector<T, U>) -> SVector<T, N> {
132        self.F * x + self.B * u
133    }
134}
135
136/// A linear system with no input.
137/// Defined by the transition matrix F and covariance matrix Q.
138#[allow(non_snake_case)]
139#[derive(Debug, Clone)]
140#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
141#[cfg_attr(feature = "defmt", derive(defmt::Format))]
142pub struct LinearNoInputSystem<T: RealField, const N: usize> {
143    x: SVector<T, N>,
144    F: SMatrix<T, N, N>,
145    F_t: SMatrix<T, N, N>,
146    Q: SMatrix<T, N, N>,
147}
148
149#[allow(non_snake_case)]
150impl<T: RealField + Copy, const N: usize> LinearNoInputSystem<T, N> {
151    /// Create new [LinearNoInputSystem] from the transition matrix F and process covariance Q.
152    pub fn new(F: SMatrix<T, N, N>, Q: SMatrix<T, N, N>, x_initial: SVector<T, N>) -> Self {
153        LinearNoInputSystem {
154            x: x_initial,
155            F,
156            F_t: F.transpose(),
157            Q,
158        }
159    }
160
161    /// Set a new transition matrix, also updating the transpose
162    pub fn set_transition(&mut self, transition: SMatrix<T, N, N>) {
163        self.F_t = transition.transpose();
164        self.F = transition;
165    }
166    /// Get a mutable reference to the process covariance matrix
167    pub fn covariance_mut(&mut self) -> &mut SMatrix<T, N, N> {
168        &mut self.Q
169    }
170}
171
172impl<T: RealField + Copy, const N: usize> System<T, N, 0> for LinearNoInputSystem<T, N> {
173    fn covariance(&self) -> &SMatrix<T, N, N> {
174        &self.Q
175    }
176
177    fn state(&self) -> &SVector<T, N> {
178        &self.x
179    }
180
181    fn state_mut(&mut self) -> &mut SVector<T, N> {
182        &mut self.x
183    }
184}
185
186impl<T: RealField + Copy, const N: usize> LinearisableSystem<T, N, 0>
187    for LinearNoInputSystem<T, N>
188{
189    fn transition(&self) -> &SMatrix<T, N, N> {
190        &self.F
191    }
192    fn transition_transpose(&self) -> &SMatrix<T, N, N> {
193        &self.F_t
194    }
195}
196
197impl<T: RealField + Copy, const N: usize> NoInputSystem<T, N> for LinearNoInputSystem<T, N> {
198    fn predict(&self, x: &SVector<T, N>) -> SVector<T, N> {
199        self.F * x
200    }
201}
202
203// ========================== Non-Linear Systems ==============================
204
205/// Type returned from [StepFunction].
206#[derive(Debug, Clone)]
207#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
208#[cfg_attr(feature = "defmt", derive(defmt::Format))]
209pub struct StepReturn<T: RealField, const N: usize> {
210    /// The new state (x).
211    pub state: SVector<T, N>,
212    /// The jacobian of the transition (F).
213    pub jacobian: SMatrix<T, N, N>,
214    /// The process covariance (Q).
215    pub covariance: SMatrix<T, N, N>,
216}
217
218/// A function that takes the current state and input,
219/// returning the next state, its covariance and the jacobian.
220/// Used for the state transition in a [NonLinearSystem].
221pub type StepFunction<T, const N: usize, const U: usize> =
222    fn(SVector<T, N>, SVector<T, U>) -> StepReturn<T, N>;
223
224/// A non-linear system with an input.
225/// Defined by a [StepFunction] that performs state transition and jacobian and covariance calculation.
226#[allow(non_snake_case)]
227#[derive(Debug, Clone)]
228// #[cfg_attr(feature = "serde", derive(serde::Serialize))]
229#[cfg_attr(feature = "defmt", derive(defmt::Format))]
230pub struct NonLinearSystem<T: RealField, const N: usize, const U: usize> {
231    /// System state
232    x: SVector<T, N>,
233    /// Process Covariance, updated after step_fn() call.
234    Q: SMatrix<T, N, N>,
235    /// Jacobian, updated after jacobian() call.
236    F: SMatrix<T, N, N>,
237    /// Jacobian transpose, updated after jacobian() call.
238    F_t: SMatrix<T, N, N>,
239    /// Function that steps from current state to next with an input
240    /// Returns the new state, the jacobian and the process covariance
241    // #[cfg_attr(feature = "serde", serde(skip))]
242    step_fn: StepFunction<T, N, U>,
243}
244
245impl<T: RealField, const N: usize, const U: usize> NonLinearSystem<T, N, U> {
246    /// Create a new [NonLinearSystem] using a [StepFunction]
247    pub fn new(step_fn: StepFunction<T, N, U>, x_initial: SVector<T, N>) -> Self {
248        Self {
249            x: x_initial,
250            Q: SMatrix::zeros(),
251            F: SMatrix::zeros(),
252            F_t: SMatrix::zeros(),
253            step_fn,
254        }
255    }
256}
257
258impl<T: RealField + Copy, const N: usize, const U: usize> System<T, N, U>
259    for NonLinearSystem<T, N, U>
260{
261    fn covariance(&self) -> &SMatrix<T, N, N> {
262        &self.Q
263    }
264
265    fn state(&self) -> &SVector<T, N> {
266        &self.x
267    }
268
269    fn state_mut(&mut self) -> &mut SVector<T, N> {
270        &mut self.x
271    }
272}
273
274impl<T: RealField + Copy, const N: usize, const U: usize> LinearisableSystem<T, N, U>
275    for NonLinearSystem<T, N, U>
276{
277    fn transition(&self) -> &SMatrix<T, N, N> {
278        &self.F
279    }
280    fn transition_transpose(&self) -> &SMatrix<T, N, N> {
281        &self.F_t
282    }
283}
284
285impl<T: RealField + Copy, const N: usize, const U: usize> InputSystem<T, N, U>
286    for NonLinearSystem<T, N, U>
287{
288    fn step(&mut self, u: SVector<T, U>) -> &SVector<T, N> {
289        // Get updated state, jacobian and process covariance
290        let r = (self.step_fn)(self.x, u);
291        self.x = r.state;
292        self.F = r.jacobian;
293        self.F_t = self.F.transpose();
294        self.Q = r.covariance;
295        // Return state
296        self.state()
297    }
298
299    fn predict(&self, x: &SVector<T, N>, u: &SVector<T, U>) -> SVector<T, N> {
300        let r = (self.step_fn)(*x, *u);
301        r.state
302    }
303}
304
305/// Convert a state matrix from continuous time (A) to discrete time (F) using zero-order-hold
306pub fn zero_order_hold<T: RealField, const N: usize>(
307    state_matrix: SMatrix<T, N, N>,
308    timestep: T,
309) -> SMatrix<T, N, N> {
310    SMatrix::identity() + state_matrix * timestep
311}
kfilter/system.rs

kfilter/
system.rs
