unscented.py 文件源码

def predict(self):
        try:
            X, Wm, Wc = sigmaPoints(self.xa, self.Pa)
        except:
            warnings.warn('Encountered a matrix that is not positive definite in the sigma points calculation at the predict step')
            self.Pa = nearpd(self.Pa)
            X, Wm, Wc = sigmaPoints(self.xa, self.Pa)
        fX, x, Pxx = unscentedTransform(X, Wm, Wc, self.fa)
        x = np.asscalar(x)
        Pxx = np.asscalar(Pxx)

        Pxv = 0.
        N = np.shape(X)[1]
        for j in range(0, N):
            Pxv += Wc[j] * fX[0,j] * X[3,j]

        self.xa = np.array( ((x,), (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  )) )