rspp 0.1.7

rust probolistic programming.
Documentation 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147

use num_traits::Pow;
extern crate nalgebra as na;
use crate::tools::tool::{self, gaussian_likehood};
use na::*;
use nalgebra::{
    base::allocator::Allocator, DefaultAllocator, Dim, DimName, DimNameAdd, MatrixMN, RealField,
    Scalar,
};
extern crate docify;
use nalgebra;
use nalgebra::DimMinimum;
use rand::prelude::*;
use statrs::distribution::{Continuous, Normal};
use statrs::statistics::Distribution;
use std::f64::consts::PI;
use std::ops::Add;

/// input u=[v,yaw_rate]
fn motion_model(x_vec: &Vec<f64>, u_vec: &Vec<f64>, dt: f64) -> Matrix4x1<f64> {
    let x = Matrix4x1::<f64>::from_vec(x_vec.to_vec());
    let u = Matrix2x1::<f64>::from_vec(u_vec.to_vec());
    let mut F = Matrix4::<f64>::identity();
    F[(3, 3)] = 0.;
    let v = u[(0, 0)];
    let yaw_rate = u[(1, 0)];
    let yaw = x[(2, 0)];
    let delta_x = dt * yaw.cos();
    let delta_y = dt * yaw.sin();
    let B = Matrix4x2::new(delta_x, 0., delta_y, 0., 0., dt, 1., 0.);
    let res = F * x + B * u;
    res
}
fn cal_covariance(x: &Vec<f64>, px: &Vec<Vec<f64>>, pw: &Vec<f64>) -> Matrix4<f64> {
    let mut cov = Matrix4::<f64>::zeros();
    let np = pw.len();
    for i in 0..np {
        let pxi = &px[i];
        let dif: Vec<f64> = x.iter().zip(pxi).map(|v| v.1 - v.0).collect();
        let dx = Matrix4x1::from_vec(dif);
        let _cov = dx * dx.transpose() * pw[i];
        cov = cov + _cov;
    }
    let dot_pw: f64 = pw.iter().map(|v| v * v).sum();
    cov = cov / (1. - dot_pw);
    cov
}
pub struct Particle {
    ///控制变量方差  也可以是协方差,采样要改
    pub u_sigma: Vec<f64>,
    ///  观测 标准差
    pub observ_sigma: f64,
    /// 粒子集合
    pub px: Vec<Vec<f64>>,
    /// 粒子对应概率
    pub pw: Vec<f64>,
}

#[doc = docify::embed!("src/robotics/particle_filter.rs", test_particle)]
impl Particle {
    pub fn sample_one(&mut self, observe: &Vec<Vec<f64>>, u: &Vec<f64>, index: usize, dt: f64) {
        let u_rnd: Vec<f64> = u
            .iter()
            .zip(self.u_sigma.clone())
            .map(|v| v.0 + tool::random_from_stdnorm() * v.1)
            .collect();
        ///根据运动方程,加了概率误差的u得到新的x
        let x = motion_model(&self.px[index], &u_rnd, dt);
        let n_landmark = observe.len();
        let mut w = self.pw[index];

        for i in 0..n_landmark {
            ///x is [x,y,yaw,v] observe is [[dis,x,y]]
            let dx = x[(0, 0)] - observe[i][1];
            let dy = x[(1, 0)] - observe[i][2];
            /// 本采样点和观测xy计算距离,此外还观测了距离,2距离diff
            let predis = (dx * dx + dy * dy).sqrt();
            /// 均值zero,观测值标准差的正态分布确定diff的概率,
            ///  综合了以前这个采样的w,和本次多个路标的w
            let diff = predis - observe[i][0];
            let norm = Normal::new(0.0, self.observ_sigma).unwrap();
            w = w * norm.pdf(diff);
        }
        let clone_x = x.as_slice().to_vec();
        self.px[index] = clone_x;
        self.pw[index] = w;
    }
    pub fn est_final(&self) -> Vec<f64> {
        let mut est_x = Vec::new();
        let cols = self.px[0].len();
        for col in 0..cols {
            let col_data: Vec<f64> = self.px.iter().map(|v| v[col]).collect();
            let after_p: f64 = col_data
                .iter()
                .zip(self.pw.clone())
                .map(|v| v.0 * v.1)
                .sum();
            est_x.push(after_p);
        }
        est_x
    }
    pub fn resample(&mut self, observe: &Vec<Vec<f64>>, u: &Vec<f64>, dt: f64) -> Vec<f64> {
        let np = self.pw.len();
        for index in 0..np {
            self.sample_one(observe, u, index, dt);
        }
        let total_p: f64 = self.pw.iter().sum();
        self.pw = self.pw.iter().map(|v| v / total_p).collect();
        let x_est = self.est_final();
        cal_covariance(&x_est, &self.px, &self.pw);
        /// 得到了估计后,更新粒子集合
        let resample_index = tool::resample_cumsum(&self.pw, self.pw.len());
        let newpx: Vec<Vec<f64>> = resample_index.iter().map(|i| self.px[*i].clone()).collect();
        self.px = newpx;
        x_est
    }
}

#[test]
#[docify::export]
fn test_particle() {
    /// x===x,y,yaw v   u== v,yaw
    let mut x = vec![0., 0., 0.];
    /// dis to car ; x,y  路标的坐标是知道的,距离是测量出来的
    let landmark0 = vec![9.78363848, 10., 0.];
    let landmark1 = vec![14.48872245, 10., 10.];
    let landmark2 = vec![15.22187153, 0., 15.];
    let landmarks = vec![landmark0, landmark1, landmark2];
    /// velocity ,yaw_rate
    let u = vec![2.22781575, 0.92108407];
    let x0 = Matrix4x1::<f64>::zeros();
    let dt = 0.1;
    // let res = motion_model(x0, u, dt);
    let pw = vec![0.5, 0.5];
    let px = vec![
        vec![2.37331233, 3.27599214, 0.96209509, 2.22781575],
        vec![1.37331233, 2.27599214, 0.96209509, 1.22781575],
    ];
    let mut p = Particle {
        pw: pw,
        px: px,
        observ_sigma: 4.,
        u_sigma: vec![4., 0.48738787],
    };
    p.resample(&landmarks, &u, 0.1);
    // let x_est = vec![1.81802421, 0.7018705, 0.82897566, 2.22781575];
    // cal_covariance(x_est, px, pw);
}