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
/*
extern crate nalgebra as na;
use na::*;
use rand::prelude::*;
let a = Matrix2::new(1, 3, 2, 4);
let b = Matrix2::new(1, 3, 2, 4);
let c = a * b; 矩阵乘法
let d = a.dot(&b); 内积一个数字
let e = a.transpose();
let aa = Matrix2::new(1., 2., 3., 4.);
let f = aa.try_inverse().unwrap();
let mut rng = rand::thread_rng();
let x = na::DMatrix::<f64>::new_random(2, 3);
let x = Matrix2x3::<f64>::zeros();
let y = DMatrix::<f64>::identity(2,2);
let Q = na::DMatrix::<f64>::diagonal(&y);
#[macro_export]
macro_rules! lkf_builder {
($lkf_wrapper: ty) => {
impl $lkf_wrapper {
fn predict(&mut self) {
// projects state ahead
self.state.x = self.lk.A * self.state.x + self.lk.B*self.state.u;
// project error covariance ahead
self.lk.P = self.lk.A * self.lk.P * self.lk.A.transpose() + self.lk.Q;
}
fn update(&mut self) {
// compute Kalman gain
let mut inner_val = self.lk.H * self.lk.P * self.lk.H.transpose() + self.lk.R;
inner_val.try_inverse_mut();
self.lk.K = self.lk.P * self.lk.H.transpose() * inner_val;
// update estimate with measurement z
self.state.x = self.state.x + self.lk.K*(self.state.z - self.lk.H * self.state.x);
// update error covariance
self.lk.P = (self.lk.I - self.lk.K * self.lk.H) * self.lk.P;
}
}
}
/// https://blog.csdn.net/robert_chen1988/article/details/104877913 u+C*Z
fn gen_mvn2_from_std(rho: f64) -> (f64, f64) {
// specify mean and covariance of our multi-variate normal
let _cov = vec![1.0, rho, rho, 1.0];
let cov = OMatrix::<f64, U2, U2>::from_vec(_cov);
//之所以半边不是0因为只有两列两行,右上角就一个0就是半边0
let cho = Cholesky::<f64, U2>::new_unchecked(cov);
let z1 = random_from_stdnorm();
let z2 = random_from_stdnorm();
let _z = vec![z1, z2];
let z = OMatrix::<f64, U2, U1>::from_vec(_z);
let res = cho.l() * z;
(res[(0, 0)], res[(1, 0)])
}
*/