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

kfilter/
system.rs
