Alex Pirciu
a
Embed:
(wiki syntax)
Show/hide line numbers
filter.inl
00001 /** 00002 ****************************************************************************** 00003 * @file Filter.inl 00004 * @author RBRO/PJ-IU 00005 * @version V1.0.0 00006 * @date day-month-year 00007 * @brief This file contains the class implementations for the filter 00008 * functionality. 00009 ****************************************************************************** 00010 */ 00011 00012 template <class T, uint32_t NC> 00013 using CMeasurementType = linalg::CColVector<T,NC>; 00014 00015 /******************************************************************************/ 00016 /** @brief CIIRFilter Class constructor 00017 * 00018 * Constructor method 00019 * 00020 * @param f_A the feedback filter coefficients 00021 * @param f_B the feedforward filter coefficients 00022 */ 00023 template <class T, uint32_t NA, uint32_t NB> 00024 filter::lti::siso::CIIRFilter<T,NA,NB>::CIIRFilter(const linalg::CRowVector<T,NA>& f_A,const linalg::CRowVector<T,NB>& f_B) 00025 : m_A(f_A) 00026 , m_B(f_B) 00027 , m_Y() 00028 , m_U() 00029 { 00030 } 00031 00032 /** @brief Operator 00033 * 00034 * @param f_u reference to input data 00035 * @return the filtered output data 00036 */ 00037 template <class T, uint32_t NA, uint32_t NB> 00038 T filter::lti::siso::CIIRFilter<T,NA,NB>::operator()(T& f_u) 00039 { 00040 for (uint32_t l_idx = NB-1; l_idx > 0; --l_idx) 00041 { 00042 m_U[l_idx] = m_U[l_idx-1]; 00043 } 00044 m_U[0][0] = f_u; 00045 00046 linalg::CMatrix<T,1,1> l_y = m_B*m_U - m_A*m_Y; 00047 // T l_y = m_B*m_U - m_A*m_Y; 00048 00049 for (uint32_t l_idx = NA-1; l_idx > 0 ; --l_idx) 00050 { 00051 m_Y[l_idx] = m_Y[l_idx-1]; 00052 } 00053 m_Y[0][0] = l_y[0][0]; 00054 00055 return m_Y[0][0]; 00056 } 00057 00058 /******************************************************************************/ 00059 /** @brief CFIRFilter Class constructor 00060 * 00061 * Constructor method 00062 * 00063 * @param f_B the feedforward filter coefficients 00064 */ 00065 template <class T, uint32_t NB> 00066 filter::lti::siso::CFIRFilter<T,NB>::CFIRFilter(const linalg::CRowVector<T,NB>& f_B) 00067 : m_B(f_B), m_U() 00068 { 00069 } 00070 00071 /** @brief Operator 00072 * 00073 * @param f_u reference to the input data 00074 * @return the filtered output data 00075 */ 00076 template <class T, uint32_t NB> 00077 T filter::lti::siso::CFIRFilter<T,NB>::operator()(T& f_u) 00078 { 00079 for (uint32_t l_idx = NB-1; l_idx > 0; --l_idx) 00080 { 00081 m_U[l_idx] = m_U[l_idx-1]; 00082 } 00083 m_U[0] = f_u; 00084 00085 linalg::CMatrix<T,1,1> l_y = m_B*m_U; 00086 00087 return l_y[0][0]; 00088 } 00089 00090 /******************************************************************************/ 00091 /** @brief MeanFilter Class constructor 00092 * 00093 * Constructor method 00094 * 00095 * 00096 */ 00097 template <class T, uint32_t NB> 00098 filter::lti::siso::CMeanFilter<T,NB>::CMeanFilter() 00099 : m_B(1./NB) 00100 , m_U() 00101 { 00102 } 00103 00104 /** @brief Operator 00105 * 00106 * @param f_u reference to the input data 00107 * @return the filtered output data 00108 */ 00109 template <class T, uint32_t NB> 00110 T filter::lti::siso::CMeanFilter<T,NB>::operator()(T& f_u) 00111 { 00112 T l_y =0; 00113 00114 for (uint32_t l_idx = 1; l_idx < NB; ++l_idx) 00115 { 00116 m_U[l_idx][0] = m_U[l_idx-1][0]; 00117 l_y += m_U[l_idx][0]; 00118 } 00119 m_U[0][0] = f_u; 00120 l_y += m_U[0][0]; 00121 00122 return m_B*l_y; 00123 } 00124 00125 /******************************************************************************/ 00126 /** @brief CSSModel Class constructor 00127 * 00128 * Constructor method 00129 * 00130 * @param f_stateTransitionMatrix 00131 * @param f_inputMatrix 00132 * @param f_measurementMatrix 00133 */ 00134 template <class T, uint32_t NA, uint32_t NB, uint32_t NC> 00135 filter::lti::mimo::CSSModel<T,NA,NB,NC>::CSSModel( 00136 const CStateTransitionType& f_stateTransitionMatrix, 00137 const CInputMatrixType& f_inputMatrix, 00138 const CMeasurementMatrixType& f_measurementMatrix) 00139 : m_stateVector() 00140 , m_stateTransitionMatrix(f_stateTransitionMatrix) 00141 , m_inputMatrix(f_inputMatrix) 00142 , m_measurementMatrix(f_measurementMatrix) 00143 , m_directTransferMatrix() 00144 { 00145 // do nothing 00146 } 00147 00148 /** @brief CSSModel Class constructor 00149 * 00150 * Constructor method 00151 * 00152 * @param f_stateTransitionMatrix 00153 * @param f_inputMatrix 00154 * @param f_measurementMatrix 00155 * @param f_directTransferMatrix 00156 */ 00157 template <class T, uint32_t NA, uint32_t NB, uint32_t NC> 00158 filter::lti::mimo::CSSModel<T,NA,NB,NC>::CSSModel( 00159 const CStateTransitionType& f_stateTransitionMatrix, 00160 const CInputMatrixType& f_inputMatrix, 00161 const CMeasurementMatrixType& f_measurementMatrix, 00162 const CDirectTransferMatrixType& f_directTransferMatrix) 00163 : m_stateVector() 00164 , m_stateTransitionMatrix(f_stateTransitionMatrix) 00165 , m_inputMatrix(f_inputMatrix) 00166 , m_measurementMatrix(f_measurementMatrix) 00167 , m_directTransferMatrix(f_directTransferMatrix) 00168 { 00169 // do nothing 00170 } 00171 00172 /** @brief CSSModel Class constructor 00173 * 00174 * Constructor method 00175 * 00176 * @param f_stateTransitionMatrix 00177 * @param f_inputMatrix 00178 * @param f_measurementMatrix 00179 * @param f_directTransferMatrix 00180 * @param f_state 00181 */ 00182 template <class T, uint32_t NA, uint32_t NB, uint32_t NC> 00183 filter::lti::mimo::CSSModel<T,NA,NB,NC>::CSSModel( 00184 const CStateTransitionType& f_stateTransitionMatrix, 00185 const CInputMatrixType& f_inputMatrix, 00186 const CMeasurementMatrixType& f_measurementMatrix, 00187 const CDirectTransferMatrixType& f_directTransferMatrix, 00188 const CStateType& f_state) 00189 : m_stateVector(f_state) 00190 , m_stateTransitionMatrix(f_stateTransitionMatrix) 00191 , m_inputMatrix(f_inputMatrix) 00192 , m_measurementMatrix(f_measurementMatrix) 00193 , m_directTransferMatrix(f_directTransferMatrix) 00194 { 00195 // do nothing 00196 } 00197 00198 /** @brief Operator 00199 * 00200 * @param f_inputVector reference to input vector 00201 * @return the output data from the system model (output vector) 00202 */ 00203 template <class T, uint32_t NA, uint32_t NB, uint32_t NC> 00204 CMeasurementType<T,NC> filter::lti::mimo::CSSModel<T,NA,NB,NC>::operator()(const CInputType& f_inputVector) 00205 { 00206 updateState(f_inputVector); 00207 00208 return getOutput(f_inputVector); 00209 } 00210 00211 /** @brief Update state method 00212 * 00213 * @param f_inputVector reference to input vector 00214 * @return None 00215 */ 00216 template <class T, uint32_t NA, uint32_t NB, uint32_t NC> 00217 void filter::lti::mimo::CSSModel<T,NA,NB,NC>::updateState(const CInputType& f_inputVector) 00218 { 00219 m_stateVector = m_stateTransitionMatrix * m_stateVector + m_inputMatrix * f_inputVector; 00220 } 00221 00222 /** @brief Get output 00223 * 00224 * @param f_inputVector reference to input vector 00225 * @return the output data from the system model (output vector) 00226 */ 00227 template <class T, uint32_t NA, uint32_t NB, uint32_t NC> 00228 CMeasurementType<T,NC> filter::lti::mimo::CSSModel<T,NA,NB,NC>::getOutput(const CInputType& f_inputVector) 00229 { 00230 return m_measurementMatrix * m_stateVector + m_directTransferMatrix * f_inputVector; 00231 } 00232 00233 /******************************************************************************/ 00234 /** @brief CKalmanFilter Class constructor 00235 * 00236 * Constructor method 00237 * 00238 * @param f_stateTransitionModel 00239 * @param f_controlInput 00240 * @param f_observationModel 00241 * @param f_covarianceProcessNoise 00242 * @param f_observationNoiseCovariance 00243 */ 00244 template <class T, uint32_t NA, uint32_t NB, uint32_t NC> 00245 filter::lti::mimo::CKalmanFilter<T,NA,NB,NC>::CKalmanFilter( 00246 const CStateTransType& f_stateTransitionModel, 00247 const CControInputType& f_controlInput, 00248 const CMeasurementType& f_observationModel, 00249 const CModelCovarianceType& f_covarianceProcessNoise, 00250 const CMeasurementCovarianceType& f_observationNoiseCovariance) 00251 : m_stateTransitionModel(f_stateTransitionModel) 00252 , m_controlInput(f_controlInput) 00253 , m_observationModel(f_observationModel) 00254 , m_covarianceProcessNoise(f_covarianceProcessNoise) 00255 , m_observationNoiseCovariance(f_observationNoiseCovariance) 00256 { 00257 } 00258 00259 /** @brief Operator 00260 * 00261 * @param f_input reference to input vector 00262 * @return Updated (a posteriori) state estimate 00263 */ 00264 template <class T, uint32_t NA, uint32_t NB, uint32_t NC> 00265 linalg::CMatrix<T, NC, NA> filter::lti::mimo::CKalmanFilter<T,NA,NB,NC>::operator()(const CInputVectorType& f_input) 00266 { 00267 predict(f_input); 00268 return update(); 00269 } 00270 00271 /** @brief Operator 00272 * 00273 * 00274 * @return Updated (a posteriori) state estimate 00275 */ 00276 template <class T, uint32_t NA, uint32_t NB, uint32_t NC> 00277 linalg::CMatrix<T, NC, NA> filter::lti::mimo::CKalmanFilter<T,NA,NB,NC>::operator()() 00278 { 00279 predict(); 00280 return update(); 00281 } 00282 00283 /** @brief Predict 00284 * 00285 * 00286 * 00287 */ 00288 template <class T, uint32_t NA, uint32_t NB, uint32_t NC> 00289 void filter::lti::mimo::CKalmanFilter<T,NA,NB,NC>::predict() 00290 { 00291 m_previousState = m_prioriState; 00292 m_previousCovariance = m_prioriCovariance; 00293 m_prioriState = m_stateTransitionModel * m_previousState; 00294 m_prioriCovariance = m_stateTransitionModel * m_previousCovariance * transpose(m_stateTransitionModel) + m_covarianceProcessNoise; 00295 } 00296 00297 /** @brief Predict 00298 * 00299 * @param f_input vector input 00300 * 00301 */ 00302 template <class T, uint32_t NA, uint32_t NB, uint32_t NC> 00303 void filter::lti::mimo::CKalmanFilter<T,NA,NB,NC>::predict(const CInputVectorType& f_input) 00304 { 00305 m_previousState = m_prioriState; 00306 m_previousCovariance = m_prioriCovariance; 00307 m_prioriState = m_stateTransitionModel * m_previousState + m_controlInput * f_input; 00308 m_prioriCovariance = m_stateTransitionModel * m_previousCovariance * transpose(m_stateTransitionModel) + m_covarianceProcessNoise; 00309 } 00310 00311 /** @brief Update 00312 * 00313 * 00314 * @return Measurement residual 00315 */ 00316 template <class T, uint32_t NA, uint32_t NB, uint32_t NC> 00317 const linalg::CMatrix<T, NC, NA>& filter::lti::mimo::CKalmanFilter<T,NA,NB,NC>::update(void) 00318 { 00319 m_measurementResidual = m_measurement - m_observationModel * m_prioriState; 00320 m_measurement = m_observationModel * m_posterioriState; 00321 m_residualCovariance = m_observationModel * m_prioriCovariance * transpose(m_observationModel) + m_observationNoiseCovariance; 00322 m_kalmanGain = m_prioriCovariance * transpose(m_observationModel) * m_residualCovariance.inv(); 00323 m_posterioriState = m_prioriState + m_kalmanGain * m_measurementResidual; 00324 m_posterioriCovariance = ( CStateTransType::eye() - m_kalmanGain * m_observationModel ) * m_prioriCovariance; 00325 return m_posterioriState; 00326 } 00327 00328 /******************************************************************************/ 00329 /** @brief CEKF Class constructor 00330 * 00331 * Constructor method 00332 * 00333 * @param f_systemModel 00334 * @param f_jbMatrices 00335 * @param f_Q 00336 * @param f_R 00337 */ 00338 template <class T, uint32_t NA, uint32_t NB, uint32_t NC> 00339 filter::ltv::mimo::CEKF<T,NA,NB,NC>::CEKF( 00340 CSystemModelType& f_systemModel 00341 ,CJacobianMatricesType& f_jbMatrices 00342 ,const CJMTransitionType& f_Q 00343 ,const CObservationNoiseType& f_R) 00344 :m_systemModel(f_systemModel) 00345 ,m_jbMatrices(f_jbMatrices) 00346 ,m_covarianceMatrix(linalg::CMatrix<T,NB,NB>::ones()) 00347 ,m_Q(f_Q) 00348 ,m_R(f_R) 00349 { 00350 } 00351 00352 /** @brief Predict 00353 * 00354 * @param f_input vector input 00355 * 00356 */ 00357 template <class T, uint32_t NA, uint32_t NB, uint32_t NC> 00358 void filter::ltv::mimo::CEKF<T,NA,NB,NC>::predict(const CInputType& f_input) 00359 { 00360 //Previous updated state 00361 CStatesType l_prev_states=m_systemModel.getStates(); 00362 CJMTransitionType l_JF=m_jbMatrices.getJMTransition(l_prev_states,f_input); 00363 //Predicted state estimate X_{k|k-1} 00364 CStatesType l_pred_states=m_systemModel.update(f_input); 00365 //Predicted covariance estimate 00366 m_covarianceMatrix=l_JF*m_covarianceMatrix*l_JF.transpose()+m_Q; 00367 } 00368 00369 /** @brief Update 00370 * 00371 * @param f_input vector input 00372 * @param f_measurement vector input 00373 * 00374 */ 00375 template <class T, uint32_t NA, uint32_t NB, uint32_t NC> 00376 void filter::ltv::mimo::CEKF<T,NA,NB,NC>::update(const CInputType& f_input 00377 ,const COutputType& f_measurement) 00378 { 00379 // Estimated system output 00380 COutputType l_y_est=m_systemModel.calculateOutput(f_input); 00381 // Innovation or measurement residual 00382 COutputType l_mes_res=f_measurement-l_y_est; 00383 // Innovation (or residual) covariance 00384 CStatesType l_states=m_systemModel.getStates(); 00385 CJMObservationType l_JH=m_jbMatrices.getJMObservation(f_input,l_states); 00386 CObservationNoiseType l_s=l_JH*m_covarianceMatrix*l_JH.transpose()+m_R; 00387 //Near-optimal Kalman gain 00388 CKalmanGainType l_K=m_covarianceMatrix*l_JH.transpose()*l_s.inv(); 00389 //Updated state estimate 00390 CStatesType l_updated_states=l_states+l_K*l_mes_res; 00391 m_systemModel.setStates(l_updated_states); 00392 //Updated covariance estimate 00393 m_covarianceMatrix=(CJMTransitionType::eye()-l_K*l_JH)*m_covarianceMatrix; 00394 } 00395 00396 /******************************************************************************/ 00397 /** @brief CMedianFilter Class constructor 00398 * 00399 * Constructor method 00400 * 00401 * 00402 */ 00403 template <class T, uint32_t N> 00404 filter::nonlinear::siso::CMedianFilter<T,N>::CMedianFilter() 00405 :m_median() 00406 ,m_smallest() 00407 ,m_new(0) 00408 ,m_size(N) 00409 ,m_queue() 00410 { 00411 // for (unsigned int l_idx = 0; l_idx < N; l_idx++) 00412 // { 00413 // m_queue[l_idx] = new structura; 00414 00415 // } 00416 00417 for (unsigned int l_idx = 0; l_idx < N ; l_idx++) 00418 { 00419 m_queue[l_idx].info = 0; 00420 m_queue[l_idx].next = &(m_queue[(l_idx + 1) % N]); 00421 m_queue[l_idx].prev = &(m_queue[(N + l_idx - 1) % N]); 00422 } 00423 00424 m_new = N - 1; 00425 m_size = N; 00426 m_median = &(m_queue[m_size / 2]); 00427 m_smallest =&(m_queue[0]); 00428 } 00429 00430 /** @brief Operator 00431 * 00432 * @param f_val the input data 00433 * @return filted the data 00434 */ 00435 template <class T, uint32_t N> 00436 T filter::nonlinear::siso::CMedianFilter<T,N>::operator()(T& f_val) 00437 { 00438 m_new = (m_new + 1) % m_size; //shift new index //inca e valoarea veche 00439 /* // varianta pentru a decide valoarea mediana eficient-----eficient daca filtrul are dimensiuni mari 00440 // ->V2 start remy_structurere EXISTA CAUZE CARE NU SUNT TRATATE CORECT SAU NETRATATE COMPLET!!!!!!!!!!! 00441 if ((m_queue[m_new]->info > m_median->info) && (f_val <= m_median->info)) 00442 { 00443 if (f_val > m_median->prev->info) 00444 { 00445 m_median = m_queue[m_new]; //med=new 00446 } 00447 else 00448 { 00449 m_median = m_median->prev; // <- 00450 } 00451 } 00452 else if ((m_queue[m_new]->info < m_median->info) && (f_val > m_median->info)) 00453 { 00454 if (f_val > m_median->next->info) 00455 { 00456 m_median = m_median->next; // -> 00457 } 00458 else 00459 { 00460 m_median = m_queue[m_new]; //med=new 00461 } 00462 } 00463 else if ((m_queue[m_new]->info == m_median->info)) 00464 { 00465 if ((f_val < m_median->info)&&(f_val <= m_median->prev->info)) 00466 { 00467 m_median = m_median->prev; // <- 00468 } 00469 else if (f_val > m_median->info) 00470 { 00471 if (f_val <= m_median->next->info) 00472 { 00473 m_median = m_queue[m_new]; //med=new 00474 } 00475 else 00476 { 00477 m_median = m_median->next; // -> 00478 } 00479 } 00480 else 00481 { 00482 m_median = m_queue[m_new]; //med=new 00483 } 00484 } 00485 */ 00486 m_queue[m_new ].info = f_val; //suprascrie cea mai veche valoare 00487 00488 //ordonare dupa valoare 00489 00490 //elementul new se "scoate" din lista 00491 m_queue[m_new].prev->next = m_queue[m_new].next; //5. 00492 m_queue[m_new].next->prev = m_queue[m_new].prev; //6. 00493 00494 //update smallest value 00495 my_structure* l_i = m_smallest; 00496 if (&(m_queue[m_new]) == m_smallest) 00497 { 00498 if (m_queue[m_new].info > m_smallest->next->info) 00499 { 00500 m_smallest = m_smallest->next; 00501 l_i = m_smallest; 00502 } 00503 } 00504 else if (m_queue[m_new].info <= m_smallest->info) 00505 { 00506 l_i = m_smallest; 00507 m_smallest = &m_queue[m_new]; 00508 } 00509 00510 //cautarea locului unde trebuie sa se amplaseze noul element in lista 00511 unsigned int l_cnt = 1; 00512 if (&(m_queue[m_new]) == l_i) 00513 { 00514 l_i = l_i->next; 00515 } 00516 00517 while ((l_i->info < m_queue[m_new].info) && (l_cnt <= m_size - 1)) 00518 { 00519 l_i = l_i->next; 00520 l_cnt++; 00521 } 00522 00523 //inserarea elemntului new la locul potrivit 00524 l_i->prev->next = &m_queue[m_new]; //1. 00525 m_queue[m_new].next = l_i; //2. 00526 m_queue[m_new].prev = l_i->prev; //3. 00527 l_i->prev = &m_queue[m_new]; //4. 00528 00529 //varianta ineficienta pentru aflarea medianului cand filtrul are dimensiuni mari 00530 m_median=m_smallest; 00531 for(uint8_t iddx=0; iddx < m_size/2; ++iddx) 00532 { 00533 m_median=m_median->next; 00534 } 00535 00536 return getMedian(); 00537 } 00538 00539 /* */ 00540 /** @brief OperGet medianator 00541 * 00542 * 00543 * @return median value 00544 */ 00545 template <class T, uint32_t N> 00546 T filter::nonlinear::siso::CMedianFilter<T, N>::getMedian() 00547 { 00548 T ret_val; 00549 if (1 == m_size % 2) // daca filtrul are lungime impara 00550 { 00551 ret_val = m_median->info; 00552 } 00553 else // daca filtrul are lungime para 00554 { 00555 ret_val = 0.5*(m_median->info + m_median->prev->info); 00556 } 00557 return ret_val; 00558 }
Generated on Tue Jul 12 2022 22:40:50 by
1.7.2