unscented.py 文件源码

def __init__(self, x0, P0, Q, R, cor, f, h):        
        self.Q = Q
        self.R = R
        self.cor = cor
        self.fa = lambda col: f(col[0], col[2])
        self.ha = lambda col: h(col[0], col[1])

        Pxx = P0
        Pxv = 0.
        self.xa = np.array( ((x0,), (0.,), (0.,), (0.,)) )
        self.Pa = np.array( ((Pxx, Pxv   , 0.      , 0.      ),
                             (Pxv, self.R, 0.      , 0.      ),
                             (0. , 0.    , self.Q  , self.cor),
                             (0. , 0.    , self.cor, self.R  )) )

        self.lastobservation = np.NAN
        self.predictedobservation = np.NAN
        self.innov = np.NAN
        self.innovcov = np.NAN
        self.gain = np.NAN

        self.loglikelihood = 0.0