Alex Pirciu
a
Diff: include/Filter/filter.inl
- Revision:
- 1:ceee5a608e7c
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/include/Filter/filter.inl Thu Mar 28 07:44:42 2019 +0000
@@ -0,0 +1,558 @@
+/**
+ ******************************************************************************
+ * @file Filter.inl
+ * @author RBRO/PJ-IU
+ * @version V1.0.0
+ * @date day-month-year
+ * @brief This file contains the class implementations for the filter
+ * functionality.
+ ******************************************************************************
+ */
+
+template <class T, uint32_t NC>
+using CMeasurementType = linalg::CColVector<T,NC>;
+
+/******************************************************************************/
+/** @brief CIIRFilter Class constructor
+ *
+ * Constructor method
+ *
+ * @param f_A the feedback filter coefficients
+ * @param f_B the feedforward filter coefficients
+ */
+template <class T, uint32_t NA, uint32_t NB>
+filter::lti::siso::CIIRFilter<T,NA,NB>::CIIRFilter(const linalg::CRowVector<T,NA>& f_A,const linalg::CRowVector<T,NB>& f_B)
+ : m_A(f_A)
+ , m_B(f_B)
+ , m_Y()
+ , m_U()
+{
+}
+
+/** @brief Operator
+ *
+ * @param f_u reference to input data
+ * @return the filtered output data
+ */
+template <class T, uint32_t NA, uint32_t NB>
+T filter::lti::siso::CIIRFilter<T,NA,NB>::operator()(T& f_u)
+{
+ for (uint32_t l_idx = NB-1; l_idx > 0; --l_idx)
+ {
+ m_U[l_idx] = m_U[l_idx-1];
+ }
+ m_U[0][0] = f_u;
+
+ linalg::CMatrix<T,1,1> l_y = m_B*m_U - m_A*m_Y;
+ // T l_y = m_B*m_U - m_A*m_Y;
+
+ for (uint32_t l_idx = NA-1; l_idx > 0 ; --l_idx)
+ {
+ m_Y[l_idx] = m_Y[l_idx-1];
+ }
+ m_Y[0][0] = l_y[0][0];
+
+ return m_Y[0][0];
+}
+
+/******************************************************************************/
+/** @brief CFIRFilter Class constructor
+ *
+ * Constructor method
+ *
+ * @param f_B the feedforward filter coefficients
+ */
+template <class T, uint32_t NB>
+filter::lti::siso::CFIRFilter<T,NB>::CFIRFilter(const linalg::CRowVector<T,NB>& f_B)
+ : m_B(f_B), m_U()
+{
+}
+
+/** @brief Operator
+ *
+ * @param f_u reference to the input data
+ * @return the filtered output data
+ */
+template <class T, uint32_t NB>
+T filter::lti::siso::CFIRFilter<T,NB>::operator()(T& f_u)
+{
+ for (uint32_t l_idx = NB-1; l_idx > 0; --l_idx)
+ {
+ m_U[l_idx] = m_U[l_idx-1];
+ }
+ m_U[0] = f_u;
+
+ linalg::CMatrix<T,1,1> l_y = m_B*m_U;
+
+ return l_y[0][0];
+}
+
+/******************************************************************************/
+/** @brief MeanFilter Class constructor
+ *
+ * Constructor method
+ *
+ *
+ */
+template <class T, uint32_t NB>
+filter::lti::siso::CMeanFilter<T,NB>::CMeanFilter()
+ : m_B(1./NB)
+ , m_U()
+{
+}
+
+/** @brief Operator
+ *
+ * @param f_u reference to the input data
+ * @return the filtered output data
+ */
+template <class T, uint32_t NB>
+T filter::lti::siso::CMeanFilter<T,NB>::operator()(T& f_u)
+{
+ T l_y =0;
+
+ for (uint32_t l_idx = 1; l_idx < NB; ++l_idx)
+ {
+ m_U[l_idx][0] = m_U[l_idx-1][0];
+ l_y += m_U[l_idx][0];
+ }
+ m_U[0][0] = f_u;
+ l_y += m_U[0][0];
+
+ return m_B*l_y;
+}
+
+/******************************************************************************/
+/** @brief CSSModel Class constructor
+ *
+ * Constructor method
+ *
+ * @param f_stateTransitionMatrix
+ * @param f_inputMatrix
+ * @param f_measurementMatrix
+ */
+template <class T, uint32_t NA, uint32_t NB, uint32_t NC>
+filter::lti::mimo::CSSModel<T,NA,NB,NC>::CSSModel(
+ const CStateTransitionType& f_stateTransitionMatrix,
+ const CInputMatrixType& f_inputMatrix,
+ const CMeasurementMatrixType& f_measurementMatrix)
+ : m_stateVector()
+ , m_stateTransitionMatrix(f_stateTransitionMatrix)
+ , m_inputMatrix(f_inputMatrix)
+ , m_measurementMatrix(f_measurementMatrix)
+ , m_directTransferMatrix()
+{
+ // do nothing
+}
+
+/** @brief CSSModel Class constructor
+ *
+ * Constructor method
+ *
+ * @param f_stateTransitionMatrix
+ * @param f_inputMatrix
+ * @param f_measurementMatrix
+ * @param f_directTransferMatrix
+ */
+template <class T, uint32_t NA, uint32_t NB, uint32_t NC>
+filter::lti::mimo::CSSModel<T,NA,NB,NC>::CSSModel(
+ const CStateTransitionType& f_stateTransitionMatrix,
+ const CInputMatrixType& f_inputMatrix,
+ const CMeasurementMatrixType& f_measurementMatrix,
+ const CDirectTransferMatrixType& f_directTransferMatrix)
+ : m_stateVector()
+ , m_stateTransitionMatrix(f_stateTransitionMatrix)
+ , m_inputMatrix(f_inputMatrix)
+ , m_measurementMatrix(f_measurementMatrix)
+ , m_directTransferMatrix(f_directTransferMatrix)
+{
+ // do nothing
+}
+
+/** @brief CSSModel Class constructor
+ *
+ * Constructor method
+ *
+ * @param f_stateTransitionMatrix
+ * @param f_inputMatrix
+ * @param f_measurementMatrix
+ * @param f_directTransferMatrix
+ * @param f_state
+ */
+template <class T, uint32_t NA, uint32_t NB, uint32_t NC>
+filter::lti::mimo::CSSModel<T,NA,NB,NC>::CSSModel(
+ const CStateTransitionType& f_stateTransitionMatrix,
+ const CInputMatrixType& f_inputMatrix,
+ const CMeasurementMatrixType& f_measurementMatrix,
+ const CDirectTransferMatrixType& f_directTransferMatrix,
+ const CStateType& f_state)
+ : m_stateVector(f_state)
+ , m_stateTransitionMatrix(f_stateTransitionMatrix)
+ , m_inputMatrix(f_inputMatrix)
+ , m_measurementMatrix(f_measurementMatrix)
+ , m_directTransferMatrix(f_directTransferMatrix)
+{
+ // do nothing
+}
+
+/** @brief Operator
+ *
+ * @param f_inputVector reference to input vector
+ * @return the output data from the system model (output vector)
+ */
+template <class T, uint32_t NA, uint32_t NB, uint32_t NC>
+CMeasurementType<T,NC> filter::lti::mimo::CSSModel<T,NA,NB,NC>::operator()(const CInputType& f_inputVector)
+{
+ updateState(f_inputVector);
+
+ return getOutput(f_inputVector);
+}
+
+/** @brief Update state method
+ *
+ * @param f_inputVector reference to input vector
+ * @return None
+ */
+template <class T, uint32_t NA, uint32_t NB, uint32_t NC>
+void filter::lti::mimo::CSSModel<T,NA,NB,NC>::updateState(const CInputType& f_inputVector)
+{
+ m_stateVector = m_stateTransitionMatrix * m_stateVector + m_inputMatrix * f_inputVector;
+}
+
+/** @brief Get output
+ *
+ * @param f_inputVector reference to input vector
+ * @return the output data from the system model (output vector)
+ */
+template <class T, uint32_t NA, uint32_t NB, uint32_t NC>
+CMeasurementType<T,NC> filter::lti::mimo::CSSModel<T,NA,NB,NC>::getOutput(const CInputType& f_inputVector)
+{
+ return m_measurementMatrix * m_stateVector + m_directTransferMatrix * f_inputVector;
+}
+
+/******************************************************************************/
+/** @brief CKalmanFilter Class constructor
+ *
+ * Constructor method
+ *
+ * @param f_stateTransitionModel
+ * @param f_controlInput
+ * @param f_observationModel
+ * @param f_covarianceProcessNoise
+ * @param f_observationNoiseCovariance
+ */
+template <class T, uint32_t NA, uint32_t NB, uint32_t NC>
+filter::lti::mimo::CKalmanFilter<T,NA,NB,NC>::CKalmanFilter(
+ const CStateTransType& f_stateTransitionModel,
+ const CControInputType& f_controlInput,
+ const CMeasurementType& f_observationModel,
+ const CModelCovarianceType& f_covarianceProcessNoise,
+ const CMeasurementCovarianceType& f_observationNoiseCovariance)
+ : m_stateTransitionModel(f_stateTransitionModel)
+ , m_controlInput(f_controlInput)
+ , m_observationModel(f_observationModel)
+ , m_covarianceProcessNoise(f_covarianceProcessNoise)
+ , m_observationNoiseCovariance(f_observationNoiseCovariance)
+{
+}
+
+/** @brief Operator
+ *
+ * @param f_input reference to input vector
+ * @return Updated (a posteriori) state estimate
+ */
+template <class T, uint32_t NA, uint32_t NB, uint32_t NC>
+linalg::CMatrix<T, NC, NA> filter::lti::mimo::CKalmanFilter<T,NA,NB,NC>::operator()(const CInputVectorType& f_input)
+{
+ predict(f_input);
+ return update();
+}
+
+/** @brief Operator
+ *
+ *
+ * @return Updated (a posteriori) state estimate
+ */
+template <class T, uint32_t NA, uint32_t NB, uint32_t NC>
+linalg::CMatrix<T, NC, NA> filter::lti::mimo::CKalmanFilter<T,NA,NB,NC>::operator()()
+{
+ predict();
+ return update();
+}
+
+/** @brief Predict
+ *
+ *
+ *
+ */
+template <class T, uint32_t NA, uint32_t NB, uint32_t NC>
+void filter::lti::mimo::CKalmanFilter<T,NA,NB,NC>::predict()
+{
+ m_previousState = m_prioriState;
+ m_previousCovariance = m_prioriCovariance;
+ m_prioriState = m_stateTransitionModel * m_previousState;
+ m_prioriCovariance = m_stateTransitionModel * m_previousCovariance * transpose(m_stateTransitionModel) + m_covarianceProcessNoise;
+}
+
+/** @brief Predict
+ *
+ * @param f_input vector input
+ *
+ */
+template <class T, uint32_t NA, uint32_t NB, uint32_t NC>
+void filter::lti::mimo::CKalmanFilter<T,NA,NB,NC>::predict(const CInputVectorType& f_input)
+{
+ m_previousState = m_prioriState;
+ m_previousCovariance = m_prioriCovariance;
+ m_prioriState = m_stateTransitionModel * m_previousState + m_controlInput * f_input;
+ m_prioriCovariance = m_stateTransitionModel * m_previousCovariance * transpose(m_stateTransitionModel) + m_covarianceProcessNoise;
+}
+
+/** @brief Update
+ *
+ *
+ * @return Measurement residual
+ */
+template <class T, uint32_t NA, uint32_t NB, uint32_t NC>
+const linalg::CMatrix<T, NC, NA>& filter::lti::mimo::CKalmanFilter<T,NA,NB,NC>::update(void)
+{
+ m_measurementResidual = m_measurement - m_observationModel * m_prioriState;
+ m_measurement = m_observationModel * m_posterioriState;
+ m_residualCovariance = m_observationModel * m_prioriCovariance * transpose(m_observationModel) + m_observationNoiseCovariance;
+ m_kalmanGain = m_prioriCovariance * transpose(m_observationModel) * m_residualCovariance.inv();
+ m_posterioriState = m_prioriState + m_kalmanGain * m_measurementResidual;
+ m_posterioriCovariance = ( CStateTransType::eye() - m_kalmanGain * m_observationModel ) * m_prioriCovariance;
+ return m_posterioriState;
+}
+
+/******************************************************************************/
+/** @brief CEKF Class constructor
+ *
+ * Constructor method
+ *
+ * @param f_systemModel
+ * @param f_jbMatrices
+ * @param f_Q
+ * @param f_R
+ */
+template <class T, uint32_t NA, uint32_t NB, uint32_t NC>
+filter::ltv::mimo::CEKF<T,NA,NB,NC>::CEKF(
+ CSystemModelType& f_systemModel
+ ,CJacobianMatricesType& f_jbMatrices
+ ,const CJMTransitionType& f_Q
+ ,const CObservationNoiseType& f_R)
+ :m_systemModel(f_systemModel)
+ ,m_jbMatrices(f_jbMatrices)
+ ,m_covarianceMatrix(linalg::CMatrix<T,NB,NB>::ones())
+ ,m_Q(f_Q)
+ ,m_R(f_R)
+{
+}
+
+/** @brief Predict
+ *
+ * @param f_input vector input
+ *
+ */
+template <class T, uint32_t NA, uint32_t NB, uint32_t NC>
+void filter::ltv::mimo::CEKF<T,NA,NB,NC>::predict(const CInputType& f_input)
+{
+ //Previous updated state
+ CStatesType l_prev_states=m_systemModel.getStates();
+ CJMTransitionType l_JF=m_jbMatrices.getJMTransition(l_prev_states,f_input);
+ //Predicted state estimate X_{k|k-1}
+ CStatesType l_pred_states=m_systemModel.update(f_input);
+ //Predicted covariance estimate
+ m_covarianceMatrix=l_JF*m_covarianceMatrix*l_JF.transpose()+m_Q;
+}
+
+/** @brief Update
+ *
+ * @param f_input vector input
+ * @param f_measurement vector input
+ *
+ */
+template <class T, uint32_t NA, uint32_t NB, uint32_t NC>
+void filter::ltv::mimo::CEKF<T,NA,NB,NC>::update(const CInputType& f_input
+ ,const COutputType& f_measurement)
+{
+ // Estimated system output
+ COutputType l_y_est=m_systemModel.calculateOutput(f_input);
+ // Innovation or measurement residual
+ COutputType l_mes_res=f_measurement-l_y_est;
+ // Innovation (or residual) covariance
+ CStatesType l_states=m_systemModel.getStates();
+ CJMObservationType l_JH=m_jbMatrices.getJMObservation(f_input,l_states);
+ CObservationNoiseType l_s=l_JH*m_covarianceMatrix*l_JH.transpose()+m_R;
+ //Near-optimal Kalman gain
+ CKalmanGainType l_K=m_covarianceMatrix*l_JH.transpose()*l_s.inv();
+ //Updated state estimate
+ CStatesType l_updated_states=l_states+l_K*l_mes_res;
+ m_systemModel.setStates(l_updated_states);
+ //Updated covariance estimate
+ m_covarianceMatrix=(CJMTransitionType::eye()-l_K*l_JH)*m_covarianceMatrix;
+}
+
+/******************************************************************************/
+/** @brief CMedianFilter Class constructor
+ *
+ * Constructor method
+ *
+ *
+ */
+template <class T, uint32_t N>
+filter::nonlinear::siso::CMedianFilter<T,N>::CMedianFilter()
+ :m_median()
+ ,m_smallest()
+ ,m_new(0)
+ ,m_size(N)
+ ,m_queue()
+{
+ // for (unsigned int l_idx = 0; l_idx < N; l_idx++)
+ // {
+ // m_queue[l_idx] = new structura;
+
+ // }
+
+ for (unsigned int l_idx = 0; l_idx < N ; l_idx++)
+ {
+ m_queue[l_idx].info = 0;
+ m_queue[l_idx].next = &(m_queue[(l_idx + 1) % N]);
+ m_queue[l_idx].prev = &(m_queue[(N + l_idx - 1) % N]);
+ }
+
+ m_new = N - 1;
+ m_size = N;
+ m_median = &(m_queue[m_size / 2]);
+ m_smallest =&(m_queue[0]);
+}
+
+/** @brief Operator
+ *
+ * @param f_val the input data
+ * @return filted the data
+ */
+template <class T, uint32_t N>
+T filter::nonlinear::siso::CMedianFilter<T,N>::operator()(T& f_val)
+{
+ m_new = (m_new + 1) % m_size; //shift new index //inca e valoarea veche
+ /* // varianta pentru a decide valoarea mediana eficient-----eficient daca filtrul are dimensiuni mari
+ // ->V2 start remy_structurere EXISTA CAUZE CARE NU SUNT TRATATE CORECT SAU NETRATATE COMPLET!!!!!!!!!!!
+ if ((m_queue[m_new]->info > m_median->info) && (f_val <= m_median->info))
+ {
+ if (f_val > m_median->prev->info)
+ {
+ m_median = m_queue[m_new]; //med=new
+ }
+ else
+ {
+ m_median = m_median->prev; // <-
+ }
+ }
+ else if ((m_queue[m_new]->info < m_median->info) && (f_val > m_median->info))
+ {
+ if (f_val > m_median->next->info)
+ {
+ m_median = m_median->next; // ->
+ }
+ else
+ {
+ m_median = m_queue[m_new]; //med=new
+ }
+ }
+ else if ((m_queue[m_new]->info == m_median->info))
+ {
+ if ((f_val < m_median->info)&&(f_val <= m_median->prev->info))
+ {
+ m_median = m_median->prev; // <-
+ }
+ else if (f_val > m_median->info)
+ {
+ if (f_val <= m_median->next->info)
+ {
+ m_median = m_queue[m_new]; //med=new
+ }
+ else
+ {
+ m_median = m_median->next; // ->
+ }
+ }
+ else
+ {
+ m_median = m_queue[m_new]; //med=new
+ }
+ }
+ */
+ m_queue[m_new ].info = f_val; //suprascrie cea mai veche valoare
+
+ //ordonare dupa valoare
+
+ //elementul new se "scoate" din lista
+ m_queue[m_new].prev->next = m_queue[m_new].next; //5.
+ m_queue[m_new].next->prev = m_queue[m_new].prev; //6.
+
+ //update smallest value
+ my_structure* l_i = m_smallest;
+ if (&(m_queue[m_new]) == m_smallest)
+ {
+ if (m_queue[m_new].info > m_smallest->next->info)
+ {
+ m_smallest = m_smallest->next;
+ l_i = m_smallest;
+ }
+ }
+ else if (m_queue[m_new].info <= m_smallest->info)
+ {
+ l_i = m_smallest;
+ m_smallest = &m_queue[m_new];
+ }
+
+ //cautarea locului unde trebuie sa se amplaseze noul element in lista
+ unsigned int l_cnt = 1;
+ if (&(m_queue[m_new]) == l_i)
+ {
+ l_i = l_i->next;
+ }
+
+ while ((l_i->info < m_queue[m_new].info) && (l_cnt <= m_size - 1))
+ {
+ l_i = l_i->next;
+ l_cnt++;
+ }
+
+ //inserarea elemntului new la locul potrivit
+ l_i->prev->next = &m_queue[m_new]; //1.
+ m_queue[m_new].next = l_i; //2.
+ m_queue[m_new].prev = l_i->prev; //3.
+ l_i->prev = &m_queue[m_new]; //4.
+
+ //varianta ineficienta pentru aflarea medianului cand filtrul are dimensiuni mari
+ m_median=m_smallest;
+ for(uint8_t iddx=0; iddx < m_size/2; ++iddx)
+ {
+ m_median=m_median->next;
+ }
+
+ return getMedian();
+}
+
+/* */
+/** @brief OperGet medianator
+ *
+ *
+ * @return median value
+ */
+template <class T, uint32_t N>
+T filter::nonlinear::siso::CMedianFilter<T, N>::getMedian()
+{
+ T ret_val;
+ if (1 == m_size % 2) // daca filtrul are lungime impara
+ {
+ ret_val = m_median->info;
+ }
+ else // daca filtrul are lungime para
+ {
+ ret_val = 0.5*(m_median->info + m_median->prev->info);
+ }
+ return ret_val;
+}