ACCIS/house.hpp at main · SIOSlab/ACCIS · GitHub

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
#ifndef HOUSE_HPP
#define HOUSE_HPP

#include <filter.hpp>

//------------------------------------------------------------------------------
//  Higher-Order Unscented Estimator
//------------------------------------------------------------------------------
class house : public filter::base {

    public:

    // Tuning parameter
    const double delta;

    // Constructor
    house(double delta_ = 0) : delta(delta_) {}

    // Filter prediction
    virtual filter::dist predict(double ti, double tf,
                const filter::dist& distXi, const filter::dist& distW,
                filter::dyn& f);

    // Filter update
    virtual filter::dist update(double ti, cvec<> z, const filter::dist& distX,
                const filter::dist& distW, filter::meas& h);

    // Joint distribution from two independent distributions
    virtual filter::dist join(const filter::dist& dist1,
                const filter::dist& dist2);

    // Marginal distribution for distribution components
    virtual filter::dist marginal(const filter::dist& joint_dist, int ind,
                int dim);

    // Distribution from points & weights
    static filter::dist get_dist(cmat<> X, cvec<> w);

    // Matrix square root
    static mat<> mat_sqrt(cmat<> A);

    // Solve sqrt(A)X = B for X
    static mat<> mat_sqrt_solve(cmat<> A, cmat<> B);

    //--------------------------------------------------------------------------
    //  Sigma point structure
    //--------------------------------------------------------------------------
    struct sig {

        // Dimensions of state & noise
        int nx, nw;

        // Number of sigma points
        int n;

        // State sigma points
        mat<> X;

        // Noise sigma points
        mat<> W;

        // Weights
        vec<> w;

        // Constructor
        sig(const filter::dist& distX, const filter::dist& distW, double delta);

    };

};

#endif