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)
