multilateration/

lib.rs

//! [![github]](https://github.com/ebakoba/multilateration) [![crates-io]](https://crates.io/crates/multilateration) [![docs-rs]](https://docs.rs/multilateration)
//!
//! [github]: https://img.shields.io/badge/github-8da0cb?style=for-the-badge&labelColor=555555&logo=github
//! [crates-io]: https://img.shields.io/badge/crates.io-fc8d62?style=for-the-badge&labelColor=555555&logo=rust
//! [docs-rs]: https://img.shields.io/badge/docs.rs-66c2a5?style=for-the-badge&labelColor=555555&logoColor=white&logo=data:image/svg+xml;base64,PHN2ZyByb2xlPSJpbWciIHhtbG5zPSJodHRwOi8vd3d3LnczLm9yZy8yMDAwL3N2ZyIgdmlld0JveD0iMCAwIDUxMiA1MTIiPjxwYXRoIGZpbGw9IiNmNWY1ZjUiIGQ9Ik00ODguNiAyNTAuMkwzOTIgMjE0VjEwNS41YzAtMTUtOS4zLTI4LjQtMjMuNC0zMy43bC0xMDAtMzcuNWMtOC4xLTMuMS0xNy4xLTMuMS0yNS4zIDBsLTEwMCAzNy41Yy0xNC4xIDUuMy0yMy40IDE4LjctMjMuNCAzMy43VjIxNGwtOTYuNiAzNi4yQzkuMyAyNTUuNSAwIDI2OC45IDAgMjgzLjlWMzk0YzAgMTMuNiA3LjcgMjYuMSAxOS45IDMyLjJsMTAwIDUwYzEwLjEgNS4xIDIyLjEgNS4xIDMyLjIgMGwxMDMuOS01MiAxMDMuOSA1MmMxMC4xIDUuMSAyMi4xIDUuMSAzMi4yIDBsMTAwLTUwYzEyLjItNi4xIDE5LjktMTguNiAxOS45LTMyLjJWMjgzLjljMC0xNS05LjMtMjguNC0yMy40LTMzLjd6TTM1OCAyMTQuOGwtODUgMzEuOXYtNjguMmw4NS0zN3Y3My4zek0xNTQgMTA0LjFsMTAyLTM4LjIgMTAyIDM4LjJ2LjZsLTEwMiA0MS40LTEwMi00MS40di0uNnptODQgMjkxLjFsLTg1IDQyLjV2LTc5LjFsODUtMzguOHY3NS40em0wLTExMmwtMTAyIDQxLjQtMTAyLTQxLjR2LS42bDEwMi0zOC4yIDEwMiAzOC4ydi42em0yNDAgMTEybC04NSA0Mi41di03OS4xbDg1LTM4Ljh2NzUuNHptMC0xMTJsLTEwMiA0MS40LTEwMi00MS40di0uNmwxMDItMzguMiAxMDIgMzguMnYuNnoiPjwvcGF0aD48L3N2Zz4K
//!
//! <br>
//!
//! This is a multilateration library implemented in Rust, which is loosly
//! based on Java [trilateration library](https://github.com/lemmingapex/trilateration).
//!
//!
//! <br>
//!
//! # Details
//!
//! - It exposes one function that uses a vector of `Measurement` struct as an input.
//!   Output will be the tuple struct called `Point` which contains the output coordinates
//!   as a vector in the first tuple slot
//!   
//!
//!   ```
//!   use multilateration::{multilaterate, Measurement, Point};
//!
//!   fn main() -> Result<()> {
//!     let measurements = vec![
//!       Measurement::new(Point(vec![1.0, 1.0, 1.0]), 1.0),
//!       Measurement::new(Point(vec![3.0, 1.0, 1.0]), 1.0),
//!       Measurement::new(Point(vec![2.0, 2.0, 1.0]), 1.0),
//!     ];
//!
//!     let coordinates = multilaterate(measurements).unwrap().0;
//!     println!("Coordinates are: {:?}", coordinates);
//!     Ok(())
//!   }
//!   ```
//!
//!   ```console
//!   Coordinates are: [2.0, 1.0000157132198315, 0.9943941804736127]
//!   ```
//!
//! <br>
//!
//! # Error conditions
//!
//! - Points have different dimensions
//! 
//! ```
//!   use multilateration::{multilaterate, Measurement, Point};
//!
//!   fn main() -> Result<()> {
//!     let measurements = vec![
//!       Measurement::new(Point(vec![1.0, 1.0]), 1.0),
//!       Measurement::new(Point(vec![3.0, 1.0, 1.0]), 1.0),
//!       Measurement::new(Point(vec![2.0, 2.0, 1.0]), 1.0),
//!     ];
//!
//!     let result = multilaterate(measurements);
//!     println!("Result is: {:?}", result);
//!     Ok(())
//!   }
//!   ```
//!
//!   ```console
//!   Result is: Err(All points must have the same dimensions)
//!   ```
//! - Points have no dimensions
//! 
//! ```
//!   use multilateration::{multilaterate, Measurement, Point};
//!
//!   fn main() -> Result<()> {
//!     let measurements = vec![
//!       Measurement::new(Point(vec![]), 1.0),
//!       Measurement::new(Point(vec![]), 1.0),
//!       Measurement::new(Point(vec![]), 1.0),
//!     ];
//!
//!     let result = multilaterate(measurements);
//!     println!("Result is: {:?}", result);
//!     Ok(())
//!   }
//!   ```
//!
//!   ```console
//!   Result is: Err(Points must contain at least one dimension)
//!   ```


use mathru::optimization::{
    LevenbergMarquardt,
    Optim
};
use mathru::algebra::linear::vector::vector::Vector;
use mathru::algebra::linear::matrix::matrix::Matrix;
use anyhow::{Result, anyhow, bail};

struct MultilaterationFunction {
    measurements: Vec<Measurement>,
}

impl MultilaterationFunction {
    pub fn new(measurements: Vec<Measurement>) -> MultilaterationFunction {
        MultilaterationFunction{
            measurements,
        }
    }

    pub fn estimate_intial_point(&self) -> Point {
        let position_dimensions = self.measurements[0].point.0.len();
        let number_of_measurements = self.measurements.len();

        let mut initial_position = vec![0f64; position_dimensions];
        for i in 0..number_of_measurements {
            for j in 0..position_dimensions {
                initial_position[j] = self
                    .measurements[i].point.0[j];
            }
        }
        for i in 0..position_dimensions {
            initial_position[i] /= number_of_measurements as f64;
        }

        Point(initial_position)
    }
}

impl Optim<f64> for MultilaterationFunction {
    fn eval(&self, input: &Vector<f64>) -> Vector<f64> {
        let mut result = vec![0f64; self.measurements.len()];

        for i in 0..self.measurements.len() {
            result[i] = 0f64;
            for j in 0..input.clone().convert_to_vec().len() {
                result[i] +=
                    f64::powf(
                        *input.get(j) -
                        self.measurements[i].point.0[j],
                    2f64
                );
            }
            result[i] -= f64::powf(self.measurements[i].distance, 2f64);
        }

        Vector::new_column(self.measurements.len(), result)
    }

    fn jacobian(&self, input: &Vector<f64>) -> Matrix<f64> {
        let input_length = input.clone().convert_to_vec().len();
        let data = vec![
            0f64;
            self.measurements.len() * input_length
        ];
        let mut matrix = Matrix::new(
            self.measurements.len(),
            input_length,
            data
        );

        for i in 0..self.measurements.len() {
            for j in 0..input_length {
                *matrix.get_mut(i, j) =
                    2f64 * input.get(j) - 
                    2f64 * self.measurements[i].point.0[j];
            }
        }

        matrix
    }
}

#[derive(Debug, Clone)]
pub struct Point(pub Vec<f64>);

#[derive(Debug)]
pub struct Measurement {
    point: Point,
    distance: f64
}

impl Measurement {
    pub fn new(point: Point, distance: f64) -> Measurement {
        Measurement {
            point,
            distance
        }
    }
}

fn validate_measurements(measurements: &[Measurement]) -> Result<()> {
    let point_dimensions: Vec<usize> = measurements.iter().map(|measurement| measurement.point.0.len()).collect();
    let min_length = *point_dimensions.iter().min().ok_or(anyhow!("Failed to calculate minimal dimension"))?;
    let max_length = *point_dimensions.iter().max().ok_or(anyhow!("Failed to calculate maximal dimension"))?;

    if min_length != max_length {
        bail!("All points must have the same dimensions");
    }
    if min_length < 1usize {
        bail!("Points must contain at least one dimension");
    }
    Ok(())
}

pub fn multilaterate(
    measurements: Vec<Measurement>
) -> Result<Point> {
    validate_measurements(&measurements)?;
    let multilateration_function = MultilaterationFunction::new(measurements);
    let optimization = LevenbergMarquardt::new(1000, -1f64, 1f64);
    let initial_point = multilateration_function.estimate_intial_point();
    let result = optimization.minimize(
        &multilateration_function,
        &Vector::new_column(
            initial_point.0.len(),
            initial_point.0
        )
    ).map_err(|_| anyhow!("Failed to calculate a result"))?;

    Ok(Point(result.arg().convert_to_vec()))
}

#[cfg(test)]
mod tests {
    use super::Point;
    use super::Measurement;
    use super::multilaterate;

    fn is_in_delta(
        delta: f64,
        value: f64,
        comparison: f64
    ) -> bool {
        value >= (comparison - delta) && value <= (comparison + delta)
    }

    #[test]
    fn multilat_1() {
        let measurements = vec![
            Measurement {
                point: Point(vec![5.0, -6.0]),
                distance: 8.06
            },
            Measurement {
                point: Point(vec![13.0, -15.0]),
                distance: 13.97
            },
            Measurement {
                point: Point(vec![21.0, -3.0]),
                distance: 23.32
            },
            Measurement {
                point: Point(vec![12.4, -21.2]),
                distance: 15.31
            },
        ];

        let result = multilaterate(measurements).unwrap().0;
        assert_eq!(result.len(), 2);
        let delta = 1f64;
        assert!(is_in_delta(delta, result[0], -0.6));
        assert!(is_in_delta(delta, result[1], -11.8));
    }

    #[test]
    fn multilat_2() {
        let measurements = vec![
            Measurement {
                point: Point(vec![1.0, 1.0]),
                distance: 0.5
            },
            Measurement {
                point: Point(vec![3.0, 1.0]),
                distance: 0.5
            },
            Measurement {
                point: Point(vec![2.0, 2.0]),
                distance: 0.5
            }
        ];

        let result = multilaterate(measurements).unwrap().0;
        assert_eq!(result.len(), 2);
        let delta = 0.4;
        assert!(is_in_delta(delta, result[0], 2.0));
        assert!(is_in_delta(delta, result[1], 1.0));
    }

    #[test]
    fn multilat_3() {
        let measurements = vec![
            Measurement {
                point: Point(vec![1.0, 1.0]),
                distance: 2.0
            },
            Measurement {
                point: Point(vec![3.0, 1.0]),
                distance: 2.0
            },
            Measurement {
                point: Point(vec![2.0, 2.0]),
                distance: 2.0
            }
        ];

        let result = multilaterate(measurements).unwrap().0;
        assert_eq!(result.len(), 2);
        let delta = 2.0;
        assert!(is_in_delta(delta, result[0], 2.0));
        assert!(is_in_delta(delta, result[1], 1.0));
    }

    #[test]
    fn multilat_4() {
        let measurements = vec![
            Measurement {
                point: Point(vec![1.0, 1.0]),
                distance: 1.0
            },
            Measurement {
                point: Point(vec![1.0, 1.0]),
                distance: 1.0
            },
            Measurement {
                point: Point(vec![3.0, 1.0]),
                distance: 1.0
            }
        ];

        let result = multilaterate(measurements).unwrap().0;
        assert_eq!(result.len(), 2);
        let delta = 0.5;
        assert!(is_in_delta(delta, result[0], 2.0));
        assert!(is_in_delta(delta, result[1], 1.0));
    }

    #[test]
    fn multilat_5() {
        let measurements = vec![
            Measurement {
                point: Point(vec![1.0, 1.0]),
                distance: 1.0
            },
            Measurement {
                point: Point(vec![3.0, 1.0]),
                distance: 1.0
            }
        ];

        let result = multilaterate(measurements).unwrap().0;
        assert_eq!(result.len(), 2);
        let delta = 0.5;
        assert!(is_in_delta(delta, result[0], 2.0));
        assert!(is_in_delta(delta, result[1], 1.0));
    }

    #[test]
    fn multilat_6() {
        let measurements = vec![
            Measurement {
                point: Point(vec![1.0, 1.0]),
                distance: 0.9
            },
            Measurement {
                point: Point(vec![3.0, 1.0]),
                distance: 1.0
            },
            Measurement {
                point: Point(vec![2.0, 2.0]),
                distance: 1.0
            }
        ];

        let result = multilaterate(measurements).unwrap().0;
        assert_eq!(result.len(), 2);
        let delta = 0.1;
        assert!(is_in_delta(delta, result[0], 2.0));
        assert!(is_in_delta(delta, result[1], 1.0));
    }

    #[test]
    fn multilat_7() {
        let measurements = vec![
            Measurement {
                point: Point(vec![1.0, 1.0, 1.0]),
                distance: 1.0
            },
            Measurement {
                point: Point(vec![3.0, 1.0, 1.0]),
                distance: 1.0
            },
            Measurement {
                point: Point(vec![2.0, 2.0, 1.0]),
                distance: 1.0
            }
        ];

        let result = multilaterate(measurements).unwrap().0;
        assert_eq!(result.len(), 3);
        let delta = 0.1;
        assert!(is_in_delta(delta, result[0], 2.0));
        assert!(is_in_delta(delta, result[1], 1.0));
        assert!(is_in_delta(delta, result[2], 1.0));
    }

    #[test]
    fn multilat_fails_on_different_dimension() {
        let measurements = vec![
            Measurement {
                point: Point(vec![1.0, 1.0 ]),
                distance: 1.0
            },
            Measurement {
                point: Point(vec![3.0, 1.0, 1.0]),
                distance: 1.0
            },
            Measurement {
                point: Point(vec![2.0, 1.0]),
                distance: 1.0
            }
        ];

        let result = multilaterate(measurements);
        assert!(result.is_err());
    }

    #[test]
    fn multilat_fails_on_zero_dimensions() {
        let measurements = vec![
            Measurement {
                point: Point(vec![]),
                distance: 1.0
            },
            Measurement {
                point: Point(vec![]),
                distance: 1.0
            },
            Measurement {
                point: Point(vec![]),
                distance: 1.0
            }
        ];

        let result = multilaterate(measurements);
        assert!(result.is_err());
    }
}