kalmanfilter.py 文件源码

def calculateQ(self, k):
      Q = M(self.Q(k))
      R = M(self.kf.R(k))
      H = M(self.kf.H(k))
      D = M(self.D[k-1])

      alpha = np.trace(D - R) / np.trace(H * M(self.kf.Pminus[k-1]) * H.T)
      alpha = np.asscalar(alpha)
      if np.isfinite(alpha) and alpha>0:
          alpha = np.power(alpha, self.exponent)
          alpha = max(0.0001, min(alpha, 1000.*mt.trace(R) / mt.trace(Q)))
      else:
          alpha = 0.0001
      Q = Q * alpha
      self.alpha[k] = alpha
      return Q