def iterate(self): """ This method defines the way the batch filter is going to iterate. Modify it if another iteration algorithm is desired. :return: """ if self._iteration < self._nmbrIterations: self._iteration = self._iteration + 1 xhat_0 = np.linalg.inv(self._stm_i_1).dot(self._xhat_i_1) Xbar_0 = self._Xhat_0 + xhat_0 self._xbar_0 = self._xbar_0 - xhat_0 self._Xhat_0 = np.copy(Xbar_0) self._t_i_1 = self._t_0 self._Xhat_i_1 = np.copy(Xbar_0) self._P_i_1 = np.copy(self._P_0) L = np.linalg.cholesky(self._P_0) # P = L*L^T self._xhat_i_1 = np.copy(self._xbar_0) self._Xref_i_1 = np.copy(Xbar_0) self._stm_i_1 = np.copy(self._I) self._R_i_1 = orTrans.backwardsSubstitutionInversion(L) self._b_i_1 = self._R_i_1.dot(self._xbar_0) return True else: return False
def configureFilter(self, Xbar_0, Pbar_0, t_0): """ Before computing the kalman solution, call this method. :param Xbar_0: [1-dimensional numpy array] Initial guess of the state. :param Pbar_0: [2-dimensional numpy array] A-priori covariance. :param t_0: [double] Initial time. :return: """ sequentialFilterProc.configureFilter(self, Xbar_0, Pbar_0, t_0) L = np.linalg.cholesky(Pbar_0) # P = L*L^T self._xbar_0 = np.zeros(Xbar_0.size) self._xhat_i_1 = np.copy(self._xbar_0) self._Xref_i_1 = np.copy(Xbar_0) self._I = np.eye(self._dynModel.getNmbrOfStates()) self._stm_i_1 = np.copy(self._I) self._R_i_1 = orTrans.backwardsSubstitutionInversion(L) self._b_i_1 = self._R_i_1.dot(self._xbar_0) self._xhat_vec = None self._stm_vec = None self._stm_t0_vec = None self._Xref_vec = None # # Default iterations # self._iteration = 0 # self._nmbrIterations = 1 return
def processCovariances(self, R, Q): """ Overriden method from sequentialFilterProc class. :param R: [2-dimensional numpy array] Observation covariance. :param Q: [2-dimensional numpy array] Noise covariance. :return: The inverse of the square root of R and Q. """ LR = np.linalg.cholesky(R) # R = LR*LR^T R_o = orTrans.backwardsSubstitutionInversion(LR) if Q != None: LQ = np.linalg.cholesky(Q) # Q = LQ*LQ^T Q_o = orTrans.backwardsSubstitutionInversion(LQ) else: Q_o = None return (R_o, Q_o)