minikalman 0.7.0

#![allow(clippy::type_complexity)]

use crate::extended::{ExtendedObservation, ExtendedObservationBuilder};
use core::marker::PhantomData;
use core::ops::{Index, IndexMut};

use crate::kalman::extended::{ExtendedKalman, ExtendedKalmanBuilder};
use crate::kalman::*;
use crate::matrix::{AsMatrix, AsMatrixMut, Matrix, MatrixMut};
use crate::prelude::{RowMajorSequentialData, RowMajorSequentialDataMut};
use crate::regular::{
    Control, ControlBuilder, RegularKalman, RegularKalmanBuilder, RegularObservation,
    RegularObservationBuilder,
};

pub fn make_dummy_filter() -> RegularKalman<
    3,
    f32,
    Dummy<f32, 3, 3>,
    Dummy<f32, 3, 1>,
    Dummy<f32, 3, 3>,
    Dummy<f32, 3, 3>,
    Dummy<f32, 3, 1>,
    Dummy<f32, 3, 3>,
> {
    RegularKalmanBuilder::new::<3, f32>(
        Dummy::default(),
        Dummy::default(),
        Dummy::default(),
        Dummy::default(),
        Dummy::default(),
        Dummy::default(),
    )
}

pub fn make_dummy_filter_ekf() -> ExtendedKalman<
    3,
    f32,
    Dummy<f32, 3, 3>,
    Dummy<f32, 3, 1>,
    Dummy<f32, 3, 3>,
    Dummy<f32, 3, 3>,
    Dummy<f32, 3, 1>,
    Dummy<f32, 3, 3>,
> {
    ExtendedKalmanBuilder::new::<3, f32>(
        Dummy::default(),
        Dummy::default(),
        Dummy::default(),
        Dummy::default(),
        Dummy::default(),
        Dummy::default(),
    )
}

pub fn make_dummy_control(
) -> Control<3, 2, f32, Dummy<f32, 3, 2>, Dummy<f32, 2, 1>, Dummy<f32, 2, 2>, Dummy<f32, 3, 2>> {
    ControlBuilder::new::<3, 2, f32>(
        Dummy::default(),
        Dummy::default(),
        Dummy::default(),
        Dummy::default(),
    )
}

pub fn make_dummy_observation() -> RegularObservation<
    3,
    1,
    f32,
    Dummy<f32, 1, 3>,
    Dummy<f32, 1, 1>,
    Dummy<f32, 1, 1>,
    Dummy<f32, 1, 1>,
    Dummy<f32, 1, 1>,
    Dummy<f32, 3, 1>,
    Dummy<f32, 1, 1>,
    Dummy<f32, 1, 3>,
    Dummy<f32, 3, 1>,
    Dummy<f32, 3, 3>,
> {
    RegularObservationBuilder::new::<3, 1, f32>(
        Dummy::default(),
        Dummy::default(),
        Dummy::default(),
        Dummy::default(),
        Dummy::default(),
        Dummy::default(),
        Dummy::default(),
        Dummy::default(),
        Dummy::default(),
        Dummy::default(),
    )
}

pub fn make_dummy_observation_ekf() -> ExtendedObservation<
    3,
    1,
    f32,
    Dummy<f32, 1, 3>,
    Dummy<f32, 1, 1>,
    Dummy<f32, 1, 1>,
    Dummy<f32, 1, 1>,
    Dummy<f32, 1, 1>,
    Dummy<f32, 3, 1>,
    Dummy<f32, 1, 1>,
    Dummy<f32, 1, 3>,
    Dummy<f32, 3, 1>,
    Dummy<f32, 3, 3>,
> {
    ExtendedObservationBuilder::new::<3, 1, f32>(
        Dummy::default(),
        Dummy::default(),
        Dummy::default(),
        Dummy::default(),
        Dummy::default(),
        Dummy::default(),
        Dummy::default(),
        Dummy::default(),
        Dummy::default(),
        Dummy::default(),
    )
}

/// A dummy buffer type that holds a [`DummyMatrix`]
#[derive(Default)]
pub struct Dummy<T, const R: usize, const C: usize>(pub DummyMatrix<T, R, C>);

/// A dummy matrix that is arbitrarily shaped.
#[derive(Default)]
pub struct DummyMatrix<T, const R: usize, const C: usize>([T; 9], PhantomData<T>);

impl<T, const R: usize, const C: usize> Dummy<T, R, C> {
    pub fn as_matrix(&self) -> &impl Matrix<R, C, T> {
        &self.0
    }

    pub fn as_matrix_mut(&mut self) -> &mut impl MatrixMut<R, C, T> {
        &mut self.0
    }
}

impl<T, const R: usize, const C: usize> RowMajorSequentialData<R, C, T> for Dummy<T, R, C> {
    fn as_slice(&self) -> &[T] {
        self.0.as_slice()
    }
}

impl<T, const R: usize, const C: usize> RowMajorSequentialDataMut<R, C, T> for Dummy<T, R, C> {
    fn as_mut_slice(&mut self) -> &mut [T] {
        self.0.as_mut_slice()
    }
}

impl<T, const R: usize, const C: usize> Index<usize> for Dummy<T, R, C> {
    type Output = T;

    fn index(&self, index: usize) -> &Self::Output {
        self.0.index(index)
    }
}

impl<T, const R: usize, const C: usize> IndexMut<usize> for Dummy<T, R, C> {
    fn index_mut(&mut self, index: usize) -> &mut Self::Output {
        self.0.index_mut(index)
    }
}

impl<T, const R: usize, const C: usize> RowMajorSequentialData<R, C, T> for DummyMatrix<T, R, C> {
    fn as_slice(&self) -> &[T] {
        self.0.as_ref()
    }
}

impl<T, const R: usize, const C: usize> RowMajorSequentialDataMut<R, C, T>
    for DummyMatrix<T, R, C>
{
    fn as_mut_slice(&mut self) -> &mut [T] {
        self.0.as_mut_slice()
    }
}

impl<T, const R: usize, const C: usize> Index<usize> for DummyMatrix<T, R, C> {
    type Output = T;

    fn index(&self, index: usize) -> &Self::Output {
        self.0.index(index)
    }
}

impl<T, const R: usize, const C: usize> IndexMut<usize> for DummyMatrix<T, R, C> {
    fn index_mut(&mut self, index: usize) -> &mut Self::Output {
        self.0.index_mut(index)
    }
}

impl<const ROWS: usize, const COLS: usize, T> Matrix<ROWS, COLS, T> for DummyMatrix<T, ROWS, COLS> {}

impl<const ROWS: usize, const COLS: usize, T> MatrixMut<ROWS, COLS, T>
    for DummyMatrix<T, ROWS, COLS>
{
}

impl<const STATES: usize, T> StateVector<STATES, T> for Dummy<T, STATES, 1> {}

impl<const STATES: usize, T> StateVectorMut<STATES, T> for Dummy<T, STATES, 1> {}

impl<const STATES: usize, T> StateTransitionMatrix<STATES, T> for Dummy<T, STATES, STATES> {
    type Target = DummyMatrix<T, STATES, STATES>;

    fn as_matrix(&self) -> &Self::Target {
        &self.0
    }
}

impl<const STATES: usize, T> StateTransitionMatrixMut<STATES, T> for Dummy<T, STATES, STATES> {
    type TargetMut = DummyMatrix<T, STATES, STATES>;

    fn as_matrix_mut(&mut self) -> &mut Self::TargetMut {
        &mut self.0
    }
}

impl<const STATES: usize, T> EstimateCovarianceMatrix<STATES, T> for Dummy<T, STATES, STATES> {
    type Target = DummyMatrix<T, STATES, STATES>;
    type TargetMut = DummyMatrix<T, STATES, STATES>;

    fn as_matrix(&self) -> &Self::Target {
        &self.0
    }
    fn as_matrix_mut(&mut self) -> &mut Self::TargetMut {
        &mut self.0
    }
}

impl<const STATES: usize, T> PredictedStateEstimateVector<STATES, T> for Dummy<T, STATES, 1> {}

impl<const STATES: usize, T> TemporaryStateMatrix<STATES, T> for Dummy<T, STATES, STATES> {
    type Target = DummyMatrix<T, STATES, STATES>;
    type TargetMut = DummyMatrix<T, STATES, STATES>;

    fn as_matrix(&self) -> &Self::Target {
        &self.0
    }

    fn as_matrix_mut(&mut self) -> &mut Self::TargetMut {
        &mut self.0
    }
}

impl<const CONTROLS: usize, T> ControlVector<CONTROLS, T> for Dummy<T, CONTROLS, 1> {}
impl<const CONTROLS: usize, T> ControlVectorMut<CONTROLS, T> for Dummy<T, CONTROLS, 1> {}
impl<const STATES: usize, const CONTROLS: usize, T> ControlMatrix<STATES, CONTROLS, T>
    for Dummy<T, STATES, CONTROLS>
{
    type Target = DummyMatrix<T, STATES, CONTROLS>;

    fn as_matrix(&self) -> &Self::Target {
        &self.0
    }
}

impl<const STATES: usize, const CONTROLS: usize, T> ControlMatrixMut<STATES, CONTROLS, T>
    for Dummy<T, STATES, CONTROLS>
{
    type TargetMut = DummyMatrix<T, STATES, CONTROLS>;

    fn as_matrix_mut(&mut self) -> &mut Self::TargetMut {
        &mut self.0
    }
}

impl<const STATES: usize, T> DirectProcessNoiseCovarianceMatrix<STATES, T>
    for Dummy<T, STATES, STATES>
{
    type Target = DummyMatrix<T, STATES, STATES>;

    fn as_matrix(&self) -> &Self::Target {
        &self.0
    }
}

impl<const STATES: usize, T> DirectProcessNoiseCovarianceMatrixMut<STATES, T>
    for Dummy<T, STATES, STATES>
{
    type TargetMut = DummyMatrix<T, STATES, STATES>;

    fn as_matrix_mut(&mut self) -> &mut Self::TargetMut {
        &mut self.0
    }
}

impl<const CONTROLS: usize, T> ControlProcessNoiseCovarianceMatrix<CONTROLS, T>
    for Dummy<T, CONTROLS, CONTROLS>
{
    type Target = DummyMatrix<T, CONTROLS, CONTROLS>;

    fn as_matrix(&self) -> &Self::Target {
        &self.0
    }
}

impl<const CONTROLS: usize, T> ControlProcessNoiseCovarianceMatrixMut<CONTROLS, T>
    for Dummy<T, CONTROLS, CONTROLS>
{
    type TargetMut = DummyMatrix<T, CONTROLS, CONTROLS>;

    fn as_matrix_mut(&mut self) -> &mut Self::TargetMut {
        &mut self.0
    }
}

impl<const STATES: usize, const CONTROLS: usize, T> TemporaryBQMatrix<STATES, CONTROLS, T>
    for Dummy<T, STATES, CONTROLS>
{
    type Target = DummyMatrix<T, STATES, CONTROLS>;
    type TargetMut = DummyMatrix<T, STATES, CONTROLS>;

    fn as_matrix(&self) -> &Self::Target {
        &self.0
    }

    fn as_matrix_mut(&mut self) -> &mut Self::TargetMut {
        &mut self.0
    }
}

impl<const STATES: usize, T> MeasurementVector<STATES, T> for Dummy<T, STATES, 1> {}

impl<const STATES: usize, T> MeasurementVectorMut<STATES, T> for Dummy<T, STATES, 1> {}

impl<const OBSERVATIONS: usize, const STATES: usize, T> ObservationMatrix<OBSERVATIONS, STATES, T>
    for Dummy<T, OBSERVATIONS, STATES>
{
    type Target = DummyMatrix<T, OBSERVATIONS, STATES>;

    fn as_matrix(&self) -> &Self::Target {
        &self.0
    }
}

impl<const OBSERVATIONS: usize, const STATES: usize, T>
    ObservationMatrixMut<OBSERVATIONS, STATES, T> for Dummy<T, OBSERVATIONS, STATES>
{
    type TargetMut = DummyMatrix<T, OBSERVATIONS, STATES>;

    fn as_matrix_mut(&mut self) -> &mut Self::TargetMut {
        &mut self.0
    }
}

impl<const OBSERVATIONS: usize, T> MeasurementNoiseCovarianceMatrix<OBSERVATIONS, T>
    for Dummy<T, OBSERVATIONS, OBSERVATIONS>
{
    type Target = DummyMatrix<T, OBSERVATIONS, OBSERVATIONS>;
    type TargetMut = DummyMatrix<T, OBSERVATIONS, OBSERVATIONS>;

    fn as_matrix(&self) -> &Self::TargetMut {
        &self.0
    }

    fn as_matrix_mut(&mut self) -> &mut Self::TargetMut {
        &mut self.0
    }
}

impl<const ROWS: usize, const COLS: usize, T> AsMatrix<ROWS, COLS, T> for Dummy<T, ROWS, COLS> {
    type Target = DummyMatrix<T, ROWS, COLS>;

    fn as_matrix(&self) -> &Self::Target {
        &self.0
    }
}

impl<const ROWS: usize, const COLS: usize, T> AsMatrixMut<ROWS, COLS, T> for Dummy<T, ROWS, COLS> {
    type TargetMut = DummyMatrix<T, ROWS, COLS>;

    fn as_matrix_mut(&mut self) -> &mut Self::TargetMut {
        &mut self.0
    }
}

impl<const OBSERVATIONS: usize, T> InnovationVector<OBSERVATIONS, T> for Dummy<T, OBSERVATIONS, 1> {}

impl<const OBSERVATIONS: usize, T> InnovationCovarianceMatrix<OBSERVATIONS, T>
    for Dummy<T, OBSERVATIONS, OBSERVATIONS>
{
    type Target = DummyMatrix<T, OBSERVATIONS, OBSERVATIONS>;
    type TargetMut = DummyMatrix<T, OBSERVATIONS, OBSERVATIONS>;

    fn as_matrix(&self) -> &Self::Target {
        &self.0
    }

    fn as_matrix_mut(&mut self) -> &mut Self::TargetMut {
        &mut self.0
    }
}

impl<const STATES: usize, const OBSERVATIONS: usize, T> KalmanGainMatrix<STATES, OBSERVATIONS, T>
    for Dummy<T, STATES, OBSERVATIONS>
{
    type Target = DummyMatrix<T, STATES, OBSERVATIONS>;
    type TargetMut = DummyMatrix<T, STATES, OBSERVATIONS>;

    fn as_matrix(&self) -> &Self::Target {
        &self.0
    }

    fn as_matrix_mut(&mut self) -> &mut Self::TargetMut {
        &mut self.0
    }
}

impl<const OBSERVATIONS: usize, T> TemporaryResidualCovarianceInvertedMatrix<OBSERVATIONS, T>
    for Dummy<T, OBSERVATIONS, OBSERVATIONS>
{
    type Target = DummyMatrix<T, OBSERVATIONS, OBSERVATIONS>;
    type TargetMut = DummyMatrix<T, OBSERVATIONS, OBSERVATIONS>;

    fn as_matrix(&self) -> &Self::Target {
        &self.0
    }

    fn as_matrix_mut(&mut self) -> &mut Self::TargetMut {
        &mut self.0
    }
}

impl<const OBSERVATIONS: usize, const STATES: usize, T> TemporaryHPMatrix<OBSERVATIONS, STATES, T>
    for Dummy<T, OBSERVATIONS, STATES>
{
    type Target = DummyMatrix<T, OBSERVATIONS, STATES>;
    type TargetMut = DummyMatrix<T, OBSERVATIONS, STATES>;

    fn as_matrix(&self) -> &Self::Target {
        &self.0
    }

    fn as_matrix_mut(&mut self) -> &mut Self::TargetMut {
        &mut self.0
    }
}

impl<const STATES: usize, T> TemporaryKHPMatrix<STATES, T> for Dummy<T, STATES, STATES> {
    type Target = DummyMatrix<T, STATES, STATES>;
    type TargetMut = DummyMatrix<T, STATES, STATES>;

    fn as_matrix(&self) -> &Self::Target {
        &self.0
    }

    fn as_matrix_mut(&mut self) -> &mut Self::TargetMut {
        &mut self.0
    }
}

impl<const STATES: usize, const OBSERVATIONS: usize, T> TemporaryPHTMatrix<STATES, OBSERVATIONS, T>
    for Dummy<T, STATES, OBSERVATIONS>
{
    type Target = DummyMatrix<T, STATES, OBSERVATIONS>;
    type TargetMut = DummyMatrix<T, STATES, OBSERVATIONS>;

    fn as_matrix(&self) -> &Self::Target {
        &self.0
    }

    fn as_matrix_mut(&mut self) -> &mut Self::TargetMut {
        &mut self.0
    }
}

impl<const STATES: usize, const NUM_SIGMA: usize, T> SigmaPointMatrix<STATES, NUM_SIGMA, T>
    for Dummy<T, STATES, NUM_SIGMA>
{
    type Target = DummyMatrix<T, STATES, NUM_SIGMA>;
    type TargetMut = DummyMatrix<T, STATES, NUM_SIGMA>;

    fn as_matrix(&self) -> &Self::Target {
        &self.0
    }

    fn as_matrix_mut(&mut self) -> &mut Self::TargetMut {
        &mut self.0
    }
}

impl<const NUM_SIGMA: usize, T> SigmaWeightsVector<NUM_SIGMA, T> for Dummy<T, NUM_SIGMA, 1> {}

impl<const NUM_SIGMA: usize, T> SigmaWeightsVectorMut<NUM_SIGMA, T> for Dummy<T, NUM_SIGMA, 1> {}

impl<const STATES: usize, const NUM_SIGMA: usize, T> SigmaPropagatedMatrix<STATES, NUM_SIGMA, T>
    for Dummy<T, STATES, NUM_SIGMA>
{
    type Target = DummyMatrix<T, STATES, NUM_SIGMA>;
    type TargetMut = DummyMatrix<T, STATES, NUM_SIGMA>;

    fn as_matrix(&self) -> &Self::Target {
        &self.0
    }

    fn as_matrix_mut(&mut self) -> &mut Self::TargetMut {
        &mut self.0
    }
}

impl<const OBSERVATIONS: usize, const NUM_SIGMA: usize, T>
    SigmaObservedMatrix<OBSERVATIONS, NUM_SIGMA, T> for Dummy<T, OBSERVATIONS, NUM_SIGMA>
{
    type Target = DummyMatrix<T, OBSERVATIONS, NUM_SIGMA>;
    type TargetMut = DummyMatrix<T, OBSERVATIONS, NUM_SIGMA>;

    fn as_matrix(&self) -> &Self::Target {
        &self.0
    }

    fn as_matrix_mut(&mut self) -> &mut Self::TargetMut {
        &mut self.0
    }
}

impl<const STATES: usize, const OBSERVATIONS: usize, T>
    CrossCovarianceMatrix<STATES, OBSERVATIONS, T> for Dummy<T, STATES, OBSERVATIONS>
{
    type Target = DummyMatrix<T, STATES, OBSERVATIONS>;
    type TargetMut = DummyMatrix<T, STATES, OBSERVATIONS>;

    fn as_matrix(&self) -> &Self::Target {
        &self.0
    }

    fn as_matrix_mut(&mut self) -> &mut Self::TargetMut {
        &mut self.0
    }
}

impl<const STATES: usize, T> TempSigmaPMatrix<STATES, T> for Dummy<T, STATES, STATES> {
    type Target = DummyMatrix<T, STATES, STATES>;
    type TargetMut = DummyMatrix<T, STATES, STATES>;

    fn as_matrix(&self) -> &Self::Target {
        &self.0
    }

    fn as_matrix_mut(&mut self) -> &mut Self::TargetMut {
        &mut self.0
    }
}

use crate::kalman::unscented::UnscentedKalman;
use crate::observations::unscented::UnscentedObservation;

pub fn make_dummy_filter_ukf() -> UnscentedKalman<
    3,
    7,
    f32,
    Dummy<f32, 3, 1>,
    Dummy<f32, 3, 3>,
    Dummy<f32, 3, 3>,
    Dummy<f32, 3, 1>,
    Dummy<f32, 3, 7>,
    Dummy<f32, 7, 1>,
    Dummy<f32, 3, 7>,
    Dummy<f32, 3, 3>,
> {
    UnscentedKalman::new(
        Dummy::default(),
        Dummy::default(),
        Dummy::default(),
        Dummy::default(),
        Dummy::default(),
        Dummy::default(),
        Dummy::default(),
        Dummy::default(),
        1e-3,
        2.0,
        0.0,
    )
}

#[allow(dead_code)]
pub fn make_dummy_observation_ukf() -> UnscentedObservation<
    3,
    2,
    7,
    f32,
    Dummy<f32, 2, 7>,
    Dummy<f32, 3, 2>,
    Dummy<f32, 2, 1>,
    Dummy<f32, 2, 2>,
    Dummy<f32, 2, 1>,
    Dummy<f32, 2, 2>,
    Dummy<f32, 3, 2>,
    Dummy<f32, 2, 2>,
    Dummy<f32, 3, 3>,
> {
    UnscentedObservation::new(
        Dummy::default(),
        Dummy::default(),
        Dummy::default(),
        Dummy::default(),
        Dummy::default(),
        Dummy::default(),
        Dummy::default(),
        Dummy::default(),
        Dummy::default(),
    )
}

#[cfg(test)]
mod tests {
    use super::*;
    use crate::matrix::{Matrix, MatrixMut};
    use crate::prelude::{RowMajorSequentialData, RowMajorSequentialDataMut};

    #[test]
    fn test_make_dummy_filter() {
        let filter = make_dummy_filter();
        assert_eq!(filter.states(), 3);
    }

    #[test]
    fn test_make_dummy_filter_ekf() {
        let filter = make_dummy_filter_ekf();
        assert_eq!(filter.states(), 3);
    }

    #[test]
    fn test_make_dummy_control() {
        let control = make_dummy_control();
        assert_eq!(control.states(), 3);
    }

    #[test]
    fn test_make_dummy_observation() {
        let obs = make_dummy_observation();
        assert_eq!(obs.states(), 3);
        assert_eq!(obs.observations(), 1);
    }

    #[test]
    fn test_make_dummy_observation_ekf() {
        let obs = make_dummy_observation_ekf();
        assert_eq!(obs.states(), 3);
        assert_eq!(obs.observations(), 1);
    }

    #[test]
    fn test_make_dummy_filter_ukf() {
        let filter = make_dummy_filter_ukf();
        assert_eq!(filter.states(), 3);
        assert_eq!(filter.num_sigma_points(), 7);
    }

    #[test]
    fn test_make_dummy_observation_ukf() {
        let obs = make_dummy_observation_ukf();
        assert_eq!(obs.states(), 3);
        assert_eq!(obs.observations(), 2);
    }

    #[test]
    fn test_dummy_as_matrix() {
        let dummy: Dummy<f32, 3, 3> = Dummy::default();
        let mat = dummy.as_matrix();
        assert_eq!(mat.get(0, 0), 0.0);
    }

    #[test]
    fn test_dummy_as_matrix_mut() {
        let mut dummy: Dummy<f32, 3, 3> = Dummy::default();
        let mat = dummy.as_matrix_mut();
        mat.set(1, 1, 42.0);
        assert_eq!(dummy.as_matrix().get(1, 1), 42.0);
    }

    #[test]
    fn test_dummy_matrix_default() {
        let matrix: DummyMatrix<f32, 3, 3> = DummyMatrix::default();
        assert_eq!(matrix.as_slice().len(), 9);
        for &v in matrix.as_slice() {
            assert_eq!(v, 0.0);
        }
    }

    #[test]
    fn test_dummy_matrix_row_major_data() {
        let mut matrix: DummyMatrix<f32, 3, 3> = DummyMatrix::default();
        let slice = matrix.as_mut_slice();
        slice[0] = 1.0;
        slice[4] = 2.0;
        slice[8] = 3.0;
        assert_eq!(matrix.as_slice()[0], 1.0);
        assert_eq!(matrix.as_slice()[4], 2.0);
        assert_eq!(matrix.as_slice()[8], 3.0);
    }

    #[test]
    fn test_dummy_matrix_index() {
        let mut matrix: DummyMatrix<f32, 3, 3> = DummyMatrix::default();
        matrix[5] = 99.0;
        assert_eq!(matrix[5], 99.0);
    }

    #[test]
    fn test_dummy_index() {
        let mut dummy: Dummy<f32, 3, 1> = Dummy::default();
        dummy[0] = 7.0;
        assert_eq!(dummy[0], 7.0);
    }

    #[test]
    fn test_dummy_row_major_data() {
        let mut dummy: Dummy<f32, 3, 3> = Dummy::default();
        let slice = dummy.as_mut_slice();
        slice[0] = 1.0;
        assert_eq!(dummy.as_slice()[0], 1.0);
    }

    #[test]
    fn test_dummy_matrix_trait_impl() {
        let matrix: DummyMatrix<f32, 3, 3> = DummyMatrix::default();
        assert_eq!(matrix.get(0, 0), 0.0);
    }

    #[test]
    fn test_dummy_matrix_mut_trait_impl() {
        let mut matrix: DummyMatrix<f32, 3, 3> = DummyMatrix::default();
        matrix.set(2, 2, 5.0);
        assert_eq!(matrix.get(2, 2), 5.0);
    }
}