def configureFilter(self, X_0, Pbar_0, t_0):
"""
Before computing the UKF 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:
"""
# Initial data
sequentialFilterProc.configureFilter(self, X_0, Pbar_0, t_0)
n = self._dynModel.getNmbrOfStates()
self._weights_mean = np.zeros(2*n+1)
self._weights_covariance = np.zeros(2*n+1)
# DEFAULT alpha and beta
alpha = 1.0
beta = 2.0
kappa_p = 3 - n
lambda_p = alpha**2 * (n + kappa_p) - n
self._gamma_p = np.sqrt(n + lambda_p)
self._weights_mean[0] = lambda_p / (n + lambda_p)
self._weights_covariance[0] = lambda_p / (n + lambda_p) + (1 - alpha**2 + beta)
self._weights_mean[1:] = 1.0/(2*(n + lambda_p))
self._weights_covariance[1:] = 1.0/(2*(n + lambda_p))
return
def configureFilter(self, Xbar_0, Pbar_0, t_0):
"""
Before computing the kalman solution, call this method.
:param Xref_0: [1-dimensional numpy array] Initial guess of the state.
:param xbar_0: [1-dimensional numpy array] Deviation from the initial guess (usually 0).
:param Pbar_0: [2-dimensional numpy array] A-priori covariance.
:param t_0: [double] Initial time.
:param joseph_flag: [boolean] Set to true to propagate the covariance using Joseph Formulation.
:return:
"""
sequentialFilterProc.configureFilter(self, Xbar_0, Pbar_0, t_0)
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_0 = np.copy(self._I)
self._stm_i_1 = np.copy(self._I)
self._Pbar_i_1 = np.copy(Pbar_0)
# # Default iterations
# self._iteration = 0
# self._nmbrIterations = 1
return
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 configureFilter(self, Xbar_0, Pbar_0, t_0):
"""
Before computing the kalman solution, call this method.
:param Xref_0: [1-dimensional numpy array] Initial guess of the state.
:param xbar_0: [1-dimensional numpy array] Deviation from the initial guess (usually 0).
:param Pbar_0: [2-dimensional numpy array] A-priori covariance.
:param t_0: [double] Initial time.
:param joseph_flag: [boolean] Set to true to propagate the covariance using Joseph Formulation.
:return:
"""
sequentialFilterProc.configureFilter(self, Xbar_0, Pbar_0, t_0)
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_0 = np.copy(self._I)
self._stm_i_1 = np.copy(self._I)
self._Pbar_i_1 = np.copy(Pbar_0)
# # Default iterations
# self._iteration = 0
# self._nmbrIterations = 1
return
