Alex Pirciu / Mbed 2 deprecated BFMC

a

Dependencies:   mbed mbed-rtos

include/Filter/filter.inl@1:ceee5a608e7c, 2019-03-28 (annotated)

Committer:
alexpirciu
Date:
Thu Mar 28 07:44:42 2019 +0000
Revision:
1:ceee5a608e7c
assa

Who changed what in which revision?

UserRevisionLine numberNew contents of line
alexpirciu 1:ceee5a608e7c 1 /**
alexpirciu 1:ceee5a608e7c 2 ******************************************************************************
alexpirciu 1:ceee5a608e7c 3 * @file Filter.inl
alexpirciu 1:ceee5a608e7c 4 * @author RBRO/PJ-IU
alexpirciu 1:ceee5a608e7c 5 * @version V1.0.0
alexpirciu 1:ceee5a608e7c 6 * @date day-month-year
alexpirciu 1:ceee5a608e7c 7 * @brief This file contains the class implementations for the filter
alexpirciu 1:ceee5a608e7c 8 * functionality.
alexpirciu 1:ceee5a608e7c 9 ******************************************************************************
alexpirciu 1:ceee5a608e7c 10 */
alexpirciu 1:ceee5a608e7c 11
alexpirciu 1:ceee5a608e7c 12 template <class T, uint32_t NC>
alexpirciu 1:ceee5a608e7c 13 using CMeasurementType = linalg::CColVector<T,NC>;
alexpirciu 1:ceee5a608e7c 14
alexpirciu 1:ceee5a608e7c 15 /******************************************************************************/
alexpirciu 1:ceee5a608e7c 16 /** @brief CIIRFilter Class constructor
alexpirciu 1:ceee5a608e7c 17 *
alexpirciu 1:ceee5a608e7c 18 * Constructor method
alexpirciu 1:ceee5a608e7c 19 *
alexpirciu 1:ceee5a608e7c 20 * @param f_A the feedback filter coefficients
alexpirciu 1:ceee5a608e7c 21 * @param f_B the feedforward filter coefficients
alexpirciu 1:ceee5a608e7c 22 */
alexpirciu 1:ceee5a608e7c 23 template <class T, uint32_t NA, uint32_t NB>
alexpirciu 1:ceee5a608e7c 24 filter::lti::siso::CIIRFilter<T,NA,NB>::CIIRFilter(const linalg::CRowVector<T,NA>& f_A,const linalg::CRowVector<T,NB>& f_B)
alexpirciu 1:ceee5a608e7c 25 : m_A(f_A)
alexpirciu 1:ceee5a608e7c 26 , m_B(f_B)
alexpirciu 1:ceee5a608e7c 27 , m_Y()
alexpirciu 1:ceee5a608e7c 28 , m_U()
alexpirciu 1:ceee5a608e7c 29 {
alexpirciu 1:ceee5a608e7c 30 }
alexpirciu 1:ceee5a608e7c 31
alexpirciu 1:ceee5a608e7c 32 /** @brief Operator
alexpirciu 1:ceee5a608e7c 33 *
alexpirciu 1:ceee5a608e7c 34 * @param f_u reference to input data
alexpirciu 1:ceee5a608e7c 35 * @return the filtered output data
alexpirciu 1:ceee5a608e7c 36 */
alexpirciu 1:ceee5a608e7c 37 template <class T, uint32_t NA, uint32_t NB>
alexpirciu 1:ceee5a608e7c 38 T filter::lti::siso::CIIRFilter<T,NA,NB>::operator()(T& f_u)
alexpirciu 1:ceee5a608e7c 39 {
alexpirciu 1:ceee5a608e7c 40 for (uint32_t l_idx = NB-1; l_idx > 0; --l_idx)
alexpirciu 1:ceee5a608e7c 41 {
alexpirciu 1:ceee5a608e7c 42 m_U[l_idx] = m_U[l_idx-1];
alexpirciu 1:ceee5a608e7c 43 }
alexpirciu 1:ceee5a608e7c 44 m_U[0][0] = f_u;
alexpirciu 1:ceee5a608e7c 45
alexpirciu 1:ceee5a608e7c 46 linalg::CMatrix<T,1,1> l_y = m_B*m_U - m_A*m_Y;
alexpirciu 1:ceee5a608e7c 47 // T l_y = m_B*m_U - m_A*m_Y;
alexpirciu 1:ceee5a608e7c 48
alexpirciu 1:ceee5a608e7c 49 for (uint32_t l_idx = NA-1; l_idx > 0 ; --l_idx)
alexpirciu 1:ceee5a608e7c 50 {
alexpirciu 1:ceee5a608e7c 51 m_Y[l_idx] = m_Y[l_idx-1];
alexpirciu 1:ceee5a608e7c 52 }
alexpirciu 1:ceee5a608e7c 53 m_Y[0][0] = l_y[0][0];
alexpirciu 1:ceee5a608e7c 54
alexpirciu 1:ceee5a608e7c 55 return m_Y[0][0];
alexpirciu 1:ceee5a608e7c 56 }
alexpirciu 1:ceee5a608e7c 57
alexpirciu 1:ceee5a608e7c 58 /******************************************************************************/
alexpirciu 1:ceee5a608e7c 59 /** @brief CFIRFilter Class constructor
alexpirciu 1:ceee5a608e7c 60 *
alexpirciu 1:ceee5a608e7c 61 * Constructor method
alexpirciu 1:ceee5a608e7c 62 *
alexpirciu 1:ceee5a608e7c 63 * @param f_B the feedforward filter coefficients
alexpirciu 1:ceee5a608e7c 64 */
alexpirciu 1:ceee5a608e7c 65 template <class T, uint32_t NB>
alexpirciu 1:ceee5a608e7c 66 filter::lti::siso::CFIRFilter<T,NB>::CFIRFilter(const linalg::CRowVector<T,NB>& f_B)
alexpirciu 1:ceee5a608e7c 67 : m_B(f_B), m_U()
alexpirciu 1:ceee5a608e7c 68 {
alexpirciu 1:ceee5a608e7c 69 }
alexpirciu 1:ceee5a608e7c 70
alexpirciu 1:ceee5a608e7c 71 /** @brief Operator
alexpirciu 1:ceee5a608e7c 72 *
alexpirciu 1:ceee5a608e7c 73 * @param f_u reference to the input data
alexpirciu 1:ceee5a608e7c 74 * @return the filtered output data
alexpirciu 1:ceee5a608e7c 75 */
alexpirciu 1:ceee5a608e7c 76 template <class T, uint32_t NB>
alexpirciu 1:ceee5a608e7c 77 T filter::lti::siso::CFIRFilter<T,NB>::operator()(T& f_u)
alexpirciu 1:ceee5a608e7c 78 {
alexpirciu 1:ceee5a608e7c 79 for (uint32_t l_idx = NB-1; l_idx > 0; --l_idx)
alexpirciu 1:ceee5a608e7c 80 {
alexpirciu 1:ceee5a608e7c 81 m_U[l_idx] = m_U[l_idx-1];
alexpirciu 1:ceee5a608e7c 82 }
alexpirciu 1:ceee5a608e7c 83 m_U[0] = f_u;
alexpirciu 1:ceee5a608e7c 84
alexpirciu 1:ceee5a608e7c 85 linalg::CMatrix<T,1,1> l_y = m_B*m_U;
alexpirciu 1:ceee5a608e7c 86
alexpirciu 1:ceee5a608e7c 87 return l_y[0][0];
alexpirciu 1:ceee5a608e7c 88 }
alexpirciu 1:ceee5a608e7c 89
alexpirciu 1:ceee5a608e7c 90 /******************************************************************************/
alexpirciu 1:ceee5a608e7c 91 /** @brief MeanFilter Class constructor
alexpirciu 1:ceee5a608e7c 92 *
alexpirciu 1:ceee5a608e7c 93 * Constructor method
alexpirciu 1:ceee5a608e7c 94 *
alexpirciu 1:ceee5a608e7c 95 *
alexpirciu 1:ceee5a608e7c 96 */
alexpirciu 1:ceee5a608e7c 97 template <class T, uint32_t NB>
alexpirciu 1:ceee5a608e7c 98 filter::lti::siso::CMeanFilter<T,NB>::CMeanFilter()
alexpirciu 1:ceee5a608e7c 99 : m_B(1./NB)
alexpirciu 1:ceee5a608e7c 100 , m_U()
alexpirciu 1:ceee5a608e7c 101 {
alexpirciu 1:ceee5a608e7c 102 }
alexpirciu 1:ceee5a608e7c 103
alexpirciu 1:ceee5a608e7c 104 /** @brief Operator
alexpirciu 1:ceee5a608e7c 105 *
alexpirciu 1:ceee5a608e7c 106 * @param f_u reference to the input data
alexpirciu 1:ceee5a608e7c 107 * @return the filtered output data
alexpirciu 1:ceee5a608e7c 108 */
alexpirciu 1:ceee5a608e7c 109 template <class T, uint32_t NB>
alexpirciu 1:ceee5a608e7c 110 T filter::lti::siso::CMeanFilter<T,NB>::operator()(T& f_u)
alexpirciu 1:ceee5a608e7c 111 {
alexpirciu 1:ceee5a608e7c 112 T l_y =0;
alexpirciu 1:ceee5a608e7c 113
alexpirciu 1:ceee5a608e7c 114 for (uint32_t l_idx = 1; l_idx < NB; ++l_idx)
alexpirciu 1:ceee5a608e7c 115 {
alexpirciu 1:ceee5a608e7c 116 m_U[l_idx][0] = m_U[l_idx-1][0];
alexpirciu 1:ceee5a608e7c 117 l_y += m_U[l_idx][0];
alexpirciu 1:ceee5a608e7c 118 }
alexpirciu 1:ceee5a608e7c 119 m_U[0][0] = f_u;
alexpirciu 1:ceee5a608e7c 120 l_y += m_U[0][0];
alexpirciu 1:ceee5a608e7c 121
alexpirciu 1:ceee5a608e7c 122 return m_B*l_y;
alexpirciu 1:ceee5a608e7c 123 }
alexpirciu 1:ceee5a608e7c 124
alexpirciu 1:ceee5a608e7c 125 /******************************************************************************/
alexpirciu 1:ceee5a608e7c 126 /** @brief CSSModel Class constructor
alexpirciu 1:ceee5a608e7c 127 *
alexpirciu 1:ceee5a608e7c 128 * Constructor method
alexpirciu 1:ceee5a608e7c 129 *
alexpirciu 1:ceee5a608e7c 130 * @param f_stateTransitionMatrix
alexpirciu 1:ceee5a608e7c 131 * @param f_inputMatrix
alexpirciu 1:ceee5a608e7c 132 * @param f_measurementMatrix
alexpirciu 1:ceee5a608e7c 133 */
alexpirciu 1:ceee5a608e7c 134 template <class T, uint32_t NA, uint32_t NB, uint32_t NC>
alexpirciu 1:ceee5a608e7c 135 filter::lti::mimo::CSSModel<T,NA,NB,NC>::CSSModel(
alexpirciu 1:ceee5a608e7c 136 const CStateTransitionType& f_stateTransitionMatrix,
alexpirciu 1:ceee5a608e7c 137 const CInputMatrixType& f_inputMatrix,
alexpirciu 1:ceee5a608e7c 138 const CMeasurementMatrixType& f_measurementMatrix)
alexpirciu 1:ceee5a608e7c 139 : m_stateVector()
alexpirciu 1:ceee5a608e7c 140 , m_stateTransitionMatrix(f_stateTransitionMatrix)
alexpirciu 1:ceee5a608e7c 141 , m_inputMatrix(f_inputMatrix)
alexpirciu 1:ceee5a608e7c 142 , m_measurementMatrix(f_measurementMatrix)
alexpirciu 1:ceee5a608e7c 143 , m_directTransferMatrix()
alexpirciu 1:ceee5a608e7c 144 {
alexpirciu 1:ceee5a608e7c 145 // do nothing
alexpirciu 1:ceee5a608e7c 146 }
alexpirciu 1:ceee5a608e7c 147
alexpirciu 1:ceee5a608e7c 148 /** @brief CSSModel Class constructor
alexpirciu 1:ceee5a608e7c 149 *
alexpirciu 1:ceee5a608e7c 150 * Constructor method
alexpirciu 1:ceee5a608e7c 151 *
alexpirciu 1:ceee5a608e7c 152 * @param f_stateTransitionMatrix
alexpirciu 1:ceee5a608e7c 153 * @param f_inputMatrix
alexpirciu 1:ceee5a608e7c 154 * @param f_measurementMatrix
alexpirciu 1:ceee5a608e7c 155 * @param f_directTransferMatrix
alexpirciu 1:ceee5a608e7c 156 */
alexpirciu 1:ceee5a608e7c 157 template <class T, uint32_t NA, uint32_t NB, uint32_t NC>
alexpirciu 1:ceee5a608e7c 158 filter::lti::mimo::CSSModel<T,NA,NB,NC>::CSSModel(
alexpirciu 1:ceee5a608e7c 159 const CStateTransitionType& f_stateTransitionMatrix,
alexpirciu 1:ceee5a608e7c 160 const CInputMatrixType& f_inputMatrix,
alexpirciu 1:ceee5a608e7c 161 const CMeasurementMatrixType& f_measurementMatrix,
alexpirciu 1:ceee5a608e7c 162 const CDirectTransferMatrixType& f_directTransferMatrix)
alexpirciu 1:ceee5a608e7c 163 : m_stateVector()
alexpirciu 1:ceee5a608e7c 164 , m_stateTransitionMatrix(f_stateTransitionMatrix)
alexpirciu 1:ceee5a608e7c 165 , m_inputMatrix(f_inputMatrix)
alexpirciu 1:ceee5a608e7c 166 , m_measurementMatrix(f_measurementMatrix)
alexpirciu 1:ceee5a608e7c 167 , m_directTransferMatrix(f_directTransferMatrix)
alexpirciu 1:ceee5a608e7c 168 {
alexpirciu 1:ceee5a608e7c 169 // do nothing
alexpirciu 1:ceee5a608e7c 170 }
alexpirciu 1:ceee5a608e7c 171
alexpirciu 1:ceee5a608e7c 172 /** @brief CSSModel Class constructor
alexpirciu 1:ceee5a608e7c 173 *
alexpirciu 1:ceee5a608e7c 174 * Constructor method
alexpirciu 1:ceee5a608e7c 175 *
alexpirciu 1:ceee5a608e7c 176 * @param f_stateTransitionMatrix
alexpirciu 1:ceee5a608e7c 177 * @param f_inputMatrix
alexpirciu 1:ceee5a608e7c 178 * @param f_measurementMatrix
alexpirciu 1:ceee5a608e7c 179 * @param f_directTransferMatrix
alexpirciu 1:ceee5a608e7c 180 * @param f_state
alexpirciu 1:ceee5a608e7c 181 */
alexpirciu 1:ceee5a608e7c 182 template <class T, uint32_t NA, uint32_t NB, uint32_t NC>
alexpirciu 1:ceee5a608e7c 183 filter::lti::mimo::CSSModel<T,NA,NB,NC>::CSSModel(
alexpirciu 1:ceee5a608e7c 184 const CStateTransitionType& f_stateTransitionMatrix,
alexpirciu 1:ceee5a608e7c 185 const CInputMatrixType& f_inputMatrix,
alexpirciu 1:ceee5a608e7c 186 const CMeasurementMatrixType& f_measurementMatrix,
alexpirciu 1:ceee5a608e7c 187 const CDirectTransferMatrixType& f_directTransferMatrix,
alexpirciu 1:ceee5a608e7c 188 const CStateType& f_state)
alexpirciu 1:ceee5a608e7c 189 : m_stateVector(f_state)
alexpirciu 1:ceee5a608e7c 190 , m_stateTransitionMatrix(f_stateTransitionMatrix)
alexpirciu 1:ceee5a608e7c 191 , m_inputMatrix(f_inputMatrix)
alexpirciu 1:ceee5a608e7c 192 , m_measurementMatrix(f_measurementMatrix)
alexpirciu 1:ceee5a608e7c 193 , m_directTransferMatrix(f_directTransferMatrix)
alexpirciu 1:ceee5a608e7c 194 {
alexpirciu 1:ceee5a608e7c 195 // do nothing
alexpirciu 1:ceee5a608e7c 196 }
alexpirciu 1:ceee5a608e7c 197
alexpirciu 1:ceee5a608e7c 198 /** @brief Operator
alexpirciu 1:ceee5a608e7c 199 *
alexpirciu 1:ceee5a608e7c 200 * @param f_inputVector reference to input vector
alexpirciu 1:ceee5a608e7c 201 * @return the output data from the system model (output vector)
alexpirciu 1:ceee5a608e7c 202 */
alexpirciu 1:ceee5a608e7c 203 template <class T, uint32_t NA, uint32_t NB, uint32_t NC>
alexpirciu 1:ceee5a608e7c 204 CMeasurementType<T,NC> filter::lti::mimo::CSSModel<T,NA,NB,NC>::operator()(const CInputType& f_inputVector)
alexpirciu 1:ceee5a608e7c 205 {
alexpirciu 1:ceee5a608e7c 206 updateState(f_inputVector);
alexpirciu 1:ceee5a608e7c 207
alexpirciu 1:ceee5a608e7c 208 return getOutput(f_inputVector);
alexpirciu 1:ceee5a608e7c 209 }
alexpirciu 1:ceee5a608e7c 210
alexpirciu 1:ceee5a608e7c 211 /** @brief Update state method
alexpirciu 1:ceee5a608e7c 212 *
alexpirciu 1:ceee5a608e7c 213 * @param f_inputVector reference to input vector
alexpirciu 1:ceee5a608e7c 214 * @return None
alexpirciu 1:ceee5a608e7c 215 */
alexpirciu 1:ceee5a608e7c 216 template <class T, uint32_t NA, uint32_t NB, uint32_t NC>
alexpirciu 1:ceee5a608e7c 217 void filter::lti::mimo::CSSModel<T,NA,NB,NC>::updateState(const CInputType& f_inputVector)
alexpirciu 1:ceee5a608e7c 218 {
alexpirciu 1:ceee5a608e7c 219 m_stateVector = m_stateTransitionMatrix * m_stateVector + m_inputMatrix * f_inputVector;
alexpirciu 1:ceee5a608e7c 220 }
alexpirciu 1:ceee5a608e7c 221
alexpirciu 1:ceee5a608e7c 222 /** @brief Get output
alexpirciu 1:ceee5a608e7c 223 *
alexpirciu 1:ceee5a608e7c 224 * @param f_inputVector reference to input vector
alexpirciu 1:ceee5a608e7c 225 * @return the output data from the system model (output vector)
alexpirciu 1:ceee5a608e7c 226 */
alexpirciu 1:ceee5a608e7c 227 template <class T, uint32_t NA, uint32_t NB, uint32_t NC>
alexpirciu 1:ceee5a608e7c 228 CMeasurementType<T,NC> filter::lti::mimo::CSSModel<T,NA,NB,NC>::getOutput(const CInputType& f_inputVector)
alexpirciu 1:ceee5a608e7c 229 {
alexpirciu 1:ceee5a608e7c 230 return m_measurementMatrix * m_stateVector + m_directTransferMatrix * f_inputVector;
alexpirciu 1:ceee5a608e7c 231 }
alexpirciu 1:ceee5a608e7c 232
alexpirciu 1:ceee5a608e7c 233 /******************************************************************************/
alexpirciu 1:ceee5a608e7c 234 /** @brief CKalmanFilter Class constructor
alexpirciu 1:ceee5a608e7c 235 *
alexpirciu 1:ceee5a608e7c 236 * Constructor method
alexpirciu 1:ceee5a608e7c 237 *
alexpirciu 1:ceee5a608e7c 238 * @param f_stateTransitionModel
alexpirciu 1:ceee5a608e7c 239 * @param f_controlInput
alexpirciu 1:ceee5a608e7c 240 * @param f_observationModel
alexpirciu 1:ceee5a608e7c 241 * @param f_covarianceProcessNoise
alexpirciu 1:ceee5a608e7c 242 * @param f_observationNoiseCovariance
alexpirciu 1:ceee5a608e7c 243 */
alexpirciu 1:ceee5a608e7c 244 template <class T, uint32_t NA, uint32_t NB, uint32_t NC>
alexpirciu 1:ceee5a608e7c 245 filter::lti::mimo::CKalmanFilter<T,NA,NB,NC>::CKalmanFilter(
alexpirciu 1:ceee5a608e7c 246 const CStateTransType& f_stateTransitionModel,
alexpirciu 1:ceee5a608e7c 247 const CControInputType& f_controlInput,
alexpirciu 1:ceee5a608e7c 248 const CMeasurementType& f_observationModel,
alexpirciu 1:ceee5a608e7c 249 const CModelCovarianceType& f_covarianceProcessNoise,
alexpirciu 1:ceee5a608e7c 250 const CMeasurementCovarianceType& f_observationNoiseCovariance)
alexpirciu 1:ceee5a608e7c 251 : m_stateTransitionModel(f_stateTransitionModel)
alexpirciu 1:ceee5a608e7c 252 , m_controlInput(f_controlInput)
alexpirciu 1:ceee5a608e7c 253 , m_observationModel(f_observationModel)
alexpirciu 1:ceee5a608e7c 254 , m_covarianceProcessNoise(f_covarianceProcessNoise)
alexpirciu 1:ceee5a608e7c 255 , m_observationNoiseCovariance(f_observationNoiseCovariance)
alexpirciu 1:ceee5a608e7c 256 {
alexpirciu 1:ceee5a608e7c 257 }
alexpirciu 1:ceee5a608e7c 258
alexpirciu 1:ceee5a608e7c 259 /** @brief Operator
alexpirciu 1:ceee5a608e7c 260 *
alexpirciu 1:ceee5a608e7c 261 * @param f_input reference to input vector
alexpirciu 1:ceee5a608e7c 262 * @return Updated (a posteriori) state estimate
alexpirciu 1:ceee5a608e7c 263 */
alexpirciu 1:ceee5a608e7c 264 template <class T, uint32_t NA, uint32_t NB, uint32_t NC>
alexpirciu 1:ceee5a608e7c 265 linalg::CMatrix<T, NC, NA> filter::lti::mimo::CKalmanFilter<T,NA,NB,NC>::operator()(const CInputVectorType& f_input)
alexpirciu 1:ceee5a608e7c 266 {
alexpirciu 1:ceee5a608e7c 267 predict(f_input);
alexpirciu 1:ceee5a608e7c 268 return update();
alexpirciu 1:ceee5a608e7c 269 }
alexpirciu 1:ceee5a608e7c 270
alexpirciu 1:ceee5a608e7c 271 /** @brief Operator
alexpirciu 1:ceee5a608e7c 272 *
alexpirciu 1:ceee5a608e7c 273 *
alexpirciu 1:ceee5a608e7c 274 * @return Updated (a posteriori) state estimate
alexpirciu 1:ceee5a608e7c 275 */
alexpirciu 1:ceee5a608e7c 276 template <class T, uint32_t NA, uint32_t NB, uint32_t NC>
alexpirciu 1:ceee5a608e7c 277 linalg::CMatrix<T, NC, NA> filter::lti::mimo::CKalmanFilter<T,NA,NB,NC>::operator()()
alexpirciu 1:ceee5a608e7c 278 {
alexpirciu 1:ceee5a608e7c 279 predict();
alexpirciu 1:ceee5a608e7c 280 return update();
alexpirciu 1:ceee5a608e7c 281 }
alexpirciu 1:ceee5a608e7c 282
alexpirciu 1:ceee5a608e7c 283 /** @brief Predict
alexpirciu 1:ceee5a608e7c 284 *
alexpirciu 1:ceee5a608e7c 285 *
alexpirciu 1:ceee5a608e7c 286 *
alexpirciu 1:ceee5a608e7c 287 */
alexpirciu 1:ceee5a608e7c 288 template <class T, uint32_t NA, uint32_t NB, uint32_t NC>
alexpirciu 1:ceee5a608e7c 289 void filter::lti::mimo::CKalmanFilter<T,NA,NB,NC>::predict()
alexpirciu 1:ceee5a608e7c 290 {
alexpirciu 1:ceee5a608e7c 291 m_previousState = m_prioriState;
alexpirciu 1:ceee5a608e7c 292 m_previousCovariance = m_prioriCovariance;
alexpirciu 1:ceee5a608e7c 293 m_prioriState = m_stateTransitionModel * m_previousState;
alexpirciu 1:ceee5a608e7c 294 m_prioriCovariance = m_stateTransitionModel * m_previousCovariance * transpose(m_stateTransitionModel) + m_covarianceProcessNoise;
alexpirciu 1:ceee5a608e7c 295 }
alexpirciu 1:ceee5a608e7c 296
alexpirciu 1:ceee5a608e7c 297 /** @brief Predict
alexpirciu 1:ceee5a608e7c 298 *
alexpirciu 1:ceee5a608e7c 299 * @param f_input vector input
alexpirciu 1:ceee5a608e7c 300 *
alexpirciu 1:ceee5a608e7c 301 */
alexpirciu 1:ceee5a608e7c 302 template <class T, uint32_t NA, uint32_t NB, uint32_t NC>
alexpirciu 1:ceee5a608e7c 303 void filter::lti::mimo::CKalmanFilter<T,NA,NB,NC>::predict(const CInputVectorType& f_input)
alexpirciu 1:ceee5a608e7c 304 {
alexpirciu 1:ceee5a608e7c 305 m_previousState = m_prioriState;
alexpirciu 1:ceee5a608e7c 306 m_previousCovariance = m_prioriCovariance;
alexpirciu 1:ceee5a608e7c 307 m_prioriState = m_stateTransitionModel * m_previousState + m_controlInput * f_input;
alexpirciu 1:ceee5a608e7c 308 m_prioriCovariance = m_stateTransitionModel * m_previousCovariance * transpose(m_stateTransitionModel) + m_covarianceProcessNoise;
alexpirciu 1:ceee5a608e7c 309 }
alexpirciu 1:ceee5a608e7c 310
alexpirciu 1:ceee5a608e7c 311 /** @brief Update
alexpirciu 1:ceee5a608e7c 312 *
alexpirciu 1:ceee5a608e7c 313 *
alexpirciu 1:ceee5a608e7c 314 * @return Measurement residual
alexpirciu 1:ceee5a608e7c 315 */
alexpirciu 1:ceee5a608e7c 316 template <class T, uint32_t NA, uint32_t NB, uint32_t NC>
alexpirciu 1:ceee5a608e7c 317 const linalg::CMatrix<T, NC, NA>& filter::lti::mimo::CKalmanFilter<T,NA,NB,NC>::update(void)
alexpirciu 1:ceee5a608e7c 318 {
alexpirciu 1:ceee5a608e7c 319 m_measurementResidual = m_measurement - m_observationModel * m_prioriState;
alexpirciu 1:ceee5a608e7c 320 m_measurement = m_observationModel * m_posterioriState;
alexpirciu 1:ceee5a608e7c 321 m_residualCovariance = m_observationModel * m_prioriCovariance * transpose(m_observationModel) + m_observationNoiseCovariance;
alexpirciu 1:ceee5a608e7c 322 m_kalmanGain = m_prioriCovariance * transpose(m_observationModel) * m_residualCovariance.inv();
alexpirciu 1:ceee5a608e7c 323 m_posterioriState = m_prioriState + m_kalmanGain * m_measurementResidual;
alexpirciu 1:ceee5a608e7c 324 m_posterioriCovariance = ( CStateTransType::eye() - m_kalmanGain * m_observationModel ) * m_prioriCovariance;
alexpirciu 1:ceee5a608e7c 325 return m_posterioriState;
alexpirciu 1:ceee5a608e7c 326 }
alexpirciu 1:ceee5a608e7c 327
alexpirciu 1:ceee5a608e7c 328 /******************************************************************************/
alexpirciu 1:ceee5a608e7c 329 /** @brief CEKF Class constructor
alexpirciu 1:ceee5a608e7c 330 *
alexpirciu 1:ceee5a608e7c 331 * Constructor method
alexpirciu 1:ceee5a608e7c 332 *
alexpirciu 1:ceee5a608e7c 333 * @param f_systemModel
alexpirciu 1:ceee5a608e7c 334 * @param f_jbMatrices
alexpirciu 1:ceee5a608e7c 335 * @param f_Q
alexpirciu 1:ceee5a608e7c 336 * @param f_R
alexpirciu 1:ceee5a608e7c 337 */
alexpirciu 1:ceee5a608e7c 338 template <class T, uint32_t NA, uint32_t NB, uint32_t NC>
alexpirciu 1:ceee5a608e7c 339 filter::ltv::mimo::CEKF<T,NA,NB,NC>::CEKF(
alexpirciu 1:ceee5a608e7c 340 CSystemModelType& f_systemModel
alexpirciu 1:ceee5a608e7c 341 ,CJacobianMatricesType& f_jbMatrices
alexpirciu 1:ceee5a608e7c 342 ,const CJMTransitionType& f_Q
alexpirciu 1:ceee5a608e7c 343 ,const CObservationNoiseType& f_R)
alexpirciu 1:ceee5a608e7c 344 :m_systemModel(f_systemModel)
alexpirciu 1:ceee5a608e7c 345 ,m_jbMatrices(f_jbMatrices)
alexpirciu 1:ceee5a608e7c 346 ,m_covarianceMatrix(linalg::CMatrix<T,NB,NB>::ones())
alexpirciu 1:ceee5a608e7c 347 ,m_Q(f_Q)
alexpirciu 1:ceee5a608e7c 348 ,m_R(f_R)
alexpirciu 1:ceee5a608e7c 349 {
alexpirciu 1:ceee5a608e7c 350 }
alexpirciu 1:ceee5a608e7c 351
alexpirciu 1:ceee5a608e7c 352 /** @brief Predict
alexpirciu 1:ceee5a608e7c 353 *
alexpirciu 1:ceee5a608e7c 354 * @param f_input vector input
alexpirciu 1:ceee5a608e7c 355 *
alexpirciu 1:ceee5a608e7c 356 */
alexpirciu 1:ceee5a608e7c 357 template <class T, uint32_t NA, uint32_t NB, uint32_t NC>
alexpirciu 1:ceee5a608e7c 358 void filter::ltv::mimo::CEKF<T,NA,NB,NC>::predict(const CInputType& f_input)
alexpirciu 1:ceee5a608e7c 359 {
alexpirciu 1:ceee5a608e7c 360 //Previous updated state
alexpirciu 1:ceee5a608e7c 361 CStatesType l_prev_states=m_systemModel.getStates();
alexpirciu 1:ceee5a608e7c 362 CJMTransitionType l_JF=m_jbMatrices.getJMTransition(l_prev_states,f_input);
alexpirciu 1:ceee5a608e7c 363 //Predicted state estimate X_{k|k-1}
alexpirciu 1:ceee5a608e7c 364 CStatesType l_pred_states=m_systemModel.update(f_input);
alexpirciu 1:ceee5a608e7c 365 //Predicted covariance estimate
alexpirciu 1:ceee5a608e7c 366 m_covarianceMatrix=l_JF*m_covarianceMatrix*l_JF.transpose()+m_Q;
alexpirciu 1:ceee5a608e7c 367 }
alexpirciu 1:ceee5a608e7c 368
alexpirciu 1:ceee5a608e7c 369 /** @brief Update
alexpirciu 1:ceee5a608e7c 370 *
alexpirciu 1:ceee5a608e7c 371 * @param f_input vector input
alexpirciu 1:ceee5a608e7c 372 * @param f_measurement vector input
alexpirciu 1:ceee5a608e7c 373 *
alexpirciu 1:ceee5a608e7c 374 */
alexpirciu 1:ceee5a608e7c 375 template <class T, uint32_t NA, uint32_t NB, uint32_t NC>
alexpirciu 1:ceee5a608e7c 376 void filter::ltv::mimo::CEKF<T,NA,NB,NC>::update(const CInputType& f_input
alexpirciu 1:ceee5a608e7c 377 ,const COutputType& f_measurement)
alexpirciu 1:ceee5a608e7c 378 {
alexpirciu 1:ceee5a608e7c 379 // Estimated system output
alexpirciu 1:ceee5a608e7c 380 COutputType l_y_est=m_systemModel.calculateOutput(f_input);
alexpirciu 1:ceee5a608e7c 381 // Innovation or measurement residual
alexpirciu 1:ceee5a608e7c 382 COutputType l_mes_res=f_measurement-l_y_est;
alexpirciu 1:ceee5a608e7c 383 // Innovation (or residual) covariance
alexpirciu 1:ceee5a608e7c 384 CStatesType l_states=m_systemModel.getStates();
alexpirciu 1:ceee5a608e7c 385 CJMObservationType l_JH=m_jbMatrices.getJMObservation(f_input,l_states);
alexpirciu 1:ceee5a608e7c 386 CObservationNoiseType l_s=l_JH*m_covarianceMatrix*l_JH.transpose()+m_R;
alexpirciu 1:ceee5a608e7c 387 //Near-optimal Kalman gain
alexpirciu 1:ceee5a608e7c 388 CKalmanGainType l_K=m_covarianceMatrix*l_JH.transpose()*l_s.inv();
alexpirciu 1:ceee5a608e7c 389 //Updated state estimate
alexpirciu 1:ceee5a608e7c 390 CStatesType l_updated_states=l_states+l_K*l_mes_res;
alexpirciu 1:ceee5a608e7c 391 m_systemModel.setStates(l_updated_states);
alexpirciu 1:ceee5a608e7c 392 //Updated covariance estimate
alexpirciu 1:ceee5a608e7c 393 m_covarianceMatrix=(CJMTransitionType::eye()-l_K*l_JH)*m_covarianceMatrix;
alexpirciu 1:ceee5a608e7c 394 }
alexpirciu 1:ceee5a608e7c 395
alexpirciu 1:ceee5a608e7c 396 /******************************************************************************/
alexpirciu 1:ceee5a608e7c 397 /** @brief CMedianFilter Class constructor
alexpirciu 1:ceee5a608e7c 398 *
alexpirciu 1:ceee5a608e7c 399 * Constructor method
alexpirciu 1:ceee5a608e7c 400 *
alexpirciu 1:ceee5a608e7c 401 *
alexpirciu 1:ceee5a608e7c 402 */
alexpirciu 1:ceee5a608e7c 403 template <class T, uint32_t N>
alexpirciu 1:ceee5a608e7c 404 filter::nonlinear::siso::CMedianFilter<T,N>::CMedianFilter()
alexpirciu 1:ceee5a608e7c 405 :m_median()
alexpirciu 1:ceee5a608e7c 406 ,m_smallest()
alexpirciu 1:ceee5a608e7c 407 ,m_new(0)
alexpirciu 1:ceee5a608e7c 408 ,m_size(N)
alexpirciu 1:ceee5a608e7c 409 ,m_queue()
alexpirciu 1:ceee5a608e7c 410 {
alexpirciu 1:ceee5a608e7c 411 // for (unsigned int l_idx = 0; l_idx < N; l_idx++)
alexpirciu 1:ceee5a608e7c 412 // {
alexpirciu 1:ceee5a608e7c 413 // m_queue[l_idx] = new structura;
alexpirciu 1:ceee5a608e7c 414
alexpirciu 1:ceee5a608e7c 415 // }
alexpirciu 1:ceee5a608e7c 416
alexpirciu 1:ceee5a608e7c 417 for (unsigned int l_idx = 0; l_idx < N ; l_idx++)
alexpirciu 1:ceee5a608e7c 418 {
alexpirciu 1:ceee5a608e7c 419 m_queue[l_idx].info = 0;
alexpirciu 1:ceee5a608e7c 420 m_queue[l_idx].next = &(m_queue[(l_idx + 1) % N]);
alexpirciu 1:ceee5a608e7c 421 m_queue[l_idx].prev = &(m_queue[(N + l_idx - 1) % N]);
alexpirciu 1:ceee5a608e7c 422 }
alexpirciu 1:ceee5a608e7c 423
alexpirciu 1:ceee5a608e7c 424 m_new = N - 1;
alexpirciu 1:ceee5a608e7c 425 m_size = N;
alexpirciu 1:ceee5a608e7c 426 m_median = &(m_queue[m_size / 2]);
alexpirciu 1:ceee5a608e7c 427 m_smallest =&(m_queue[0]);
alexpirciu 1:ceee5a608e7c 428 }
alexpirciu 1:ceee5a608e7c 429
alexpirciu 1:ceee5a608e7c 430 /** @brief Operator
alexpirciu 1:ceee5a608e7c 431 *
alexpirciu 1:ceee5a608e7c 432 * @param f_val the input data
alexpirciu 1:ceee5a608e7c 433 * @return filted the data
alexpirciu 1:ceee5a608e7c 434 */
alexpirciu 1:ceee5a608e7c 435 template <class T, uint32_t N>
alexpirciu 1:ceee5a608e7c 436 T filter::nonlinear::siso::CMedianFilter<T,N>::operator()(T& f_val)
alexpirciu 1:ceee5a608e7c 437 {
alexpirciu 1:ceee5a608e7c 438 m_new = (m_new + 1) % m_size; //shift new index //inca e valoarea veche
alexpirciu 1:ceee5a608e7c 439 /* // varianta pentru a decide valoarea mediana eficient-----eficient daca filtrul are dimensiuni mari
alexpirciu 1:ceee5a608e7c 440 // ->V2 start remy_structurere EXISTA CAUZE CARE NU SUNT TRATATE CORECT SAU NETRATATE COMPLET!!!!!!!!!!!
alexpirciu 1:ceee5a608e7c 441 if ((m_queue[m_new]->info > m_median->info) && (f_val <= m_median->info))
alexpirciu 1:ceee5a608e7c 442 {
alexpirciu 1:ceee5a608e7c 443 if (f_val > m_median->prev->info)
alexpirciu 1:ceee5a608e7c 444 {
alexpirciu 1:ceee5a608e7c 445 m_median = m_queue[m_new]; //med=new
alexpirciu 1:ceee5a608e7c 446 }
alexpirciu 1:ceee5a608e7c 447 else
alexpirciu 1:ceee5a608e7c 448 {
alexpirciu 1:ceee5a608e7c 449 m_median = m_median->prev; // <-
alexpirciu 1:ceee5a608e7c 450 }
alexpirciu 1:ceee5a608e7c 451 }
alexpirciu 1:ceee5a608e7c 452 else if ((m_queue[m_new]->info < m_median->info) && (f_val > m_median->info))
alexpirciu 1:ceee5a608e7c 453 {
alexpirciu 1:ceee5a608e7c 454 if (f_val > m_median->next->info)
alexpirciu 1:ceee5a608e7c 455 {
alexpirciu 1:ceee5a608e7c 456 m_median = m_median->next; // ->
alexpirciu 1:ceee5a608e7c 457 }
alexpirciu 1:ceee5a608e7c 458 else
alexpirciu 1:ceee5a608e7c 459 {
alexpirciu 1:ceee5a608e7c 460 m_median = m_queue[m_new]; //med=new
alexpirciu 1:ceee5a608e7c 461 }
alexpirciu 1:ceee5a608e7c 462 }
alexpirciu 1:ceee5a608e7c 463 else if ((m_queue[m_new]->info == m_median->info))
alexpirciu 1:ceee5a608e7c 464 {
alexpirciu 1:ceee5a608e7c 465 if ((f_val < m_median->info)&&(f_val <= m_median->prev->info))
alexpirciu 1:ceee5a608e7c 466 {
alexpirciu 1:ceee5a608e7c 467 m_median = m_median->prev; // <-
alexpirciu 1:ceee5a608e7c 468 }
alexpirciu 1:ceee5a608e7c 469 else if (f_val > m_median->info)
alexpirciu 1:ceee5a608e7c 470 {
alexpirciu 1:ceee5a608e7c 471 if (f_val <= m_median->next->info)
alexpirciu 1:ceee5a608e7c 472 {
alexpirciu 1:ceee5a608e7c 473 m_median = m_queue[m_new]; //med=new
alexpirciu 1:ceee5a608e7c 474 }
alexpirciu 1:ceee5a608e7c 475 else
alexpirciu 1:ceee5a608e7c 476 {
alexpirciu 1:ceee5a608e7c 477 m_median = m_median->next; // ->
alexpirciu 1:ceee5a608e7c 478 }
alexpirciu 1:ceee5a608e7c 479 }
alexpirciu 1:ceee5a608e7c 480 else
alexpirciu 1:ceee5a608e7c 481 {
alexpirciu 1:ceee5a608e7c 482 m_median = m_queue[m_new]; //med=new
alexpirciu 1:ceee5a608e7c 483 }
alexpirciu 1:ceee5a608e7c 484 }
alexpirciu 1:ceee5a608e7c 485 */
alexpirciu 1:ceee5a608e7c 486 m_queue[m_new ].info = f_val; //suprascrie cea mai veche valoare
alexpirciu 1:ceee5a608e7c 487
alexpirciu 1:ceee5a608e7c 488 //ordonare dupa valoare
alexpirciu 1:ceee5a608e7c 489
alexpirciu 1:ceee5a608e7c 490 //elementul new se "scoate" din lista
alexpirciu 1:ceee5a608e7c 491 m_queue[m_new].prev->next = m_queue[m_new].next; //5.
alexpirciu 1:ceee5a608e7c 492 m_queue[m_new].next->prev = m_queue[m_new].prev; //6.
alexpirciu 1:ceee5a608e7c 493
alexpirciu 1:ceee5a608e7c 494 //update smallest value
alexpirciu 1:ceee5a608e7c 495 my_structure* l_i = m_smallest;
alexpirciu 1:ceee5a608e7c 496 if (&(m_queue[m_new]) == m_smallest)
alexpirciu 1:ceee5a608e7c 497 {
alexpirciu 1:ceee5a608e7c 498 if (m_queue[m_new].info > m_smallest->next->info)
alexpirciu 1:ceee5a608e7c 499 {
alexpirciu 1:ceee5a608e7c 500 m_smallest = m_smallest->next;
alexpirciu 1:ceee5a608e7c 501 l_i = m_smallest;
alexpirciu 1:ceee5a608e7c 502 }
alexpirciu 1:ceee5a608e7c 503 }
alexpirciu 1:ceee5a608e7c 504 else if (m_queue[m_new].info <= m_smallest->info)
alexpirciu 1:ceee5a608e7c 505 {
alexpirciu 1:ceee5a608e7c 506 l_i = m_smallest;
alexpirciu 1:ceee5a608e7c 507 m_smallest = &m_queue[m_new];
alexpirciu 1:ceee5a608e7c 508 }
alexpirciu 1:ceee5a608e7c 509
alexpirciu 1:ceee5a608e7c 510 //cautarea locului unde trebuie sa se amplaseze noul element in lista
alexpirciu 1:ceee5a608e7c 511 unsigned int l_cnt = 1;
alexpirciu 1:ceee5a608e7c 512 if (&(m_queue[m_new]) == l_i)
alexpirciu 1:ceee5a608e7c 513 {
alexpirciu 1:ceee5a608e7c 514 l_i = l_i->next;
alexpirciu 1:ceee5a608e7c 515 }
alexpirciu 1:ceee5a608e7c 516
alexpirciu 1:ceee5a608e7c 517 while ((l_i->info < m_queue[m_new].info) && (l_cnt <= m_size - 1))
alexpirciu 1:ceee5a608e7c 518 {
alexpirciu 1:ceee5a608e7c 519 l_i = l_i->next;
alexpirciu 1:ceee5a608e7c 520 l_cnt++;
alexpirciu 1:ceee5a608e7c 521 }
alexpirciu 1:ceee5a608e7c 522
alexpirciu 1:ceee5a608e7c 523 //inserarea elemntului new la locul potrivit
alexpirciu 1:ceee5a608e7c 524 l_i->prev->next = &m_queue[m_new]; //1.
alexpirciu 1:ceee5a608e7c 525 m_queue[m_new].next = l_i; //2.
alexpirciu 1:ceee5a608e7c 526 m_queue[m_new].prev = l_i->prev; //3.
alexpirciu 1:ceee5a608e7c 527 l_i->prev = &m_queue[m_new]; //4.
alexpirciu 1:ceee5a608e7c 528
alexpirciu 1:ceee5a608e7c 529 //varianta ineficienta pentru aflarea medianului cand filtrul are dimensiuni mari
alexpirciu 1:ceee5a608e7c 530 m_median=m_smallest;
alexpirciu 1:ceee5a608e7c 531 for(uint8_t iddx=0; iddx < m_size/2; ++iddx)
alexpirciu 1:ceee5a608e7c 532 {
alexpirciu 1:ceee5a608e7c 533 m_median=m_median->next;
alexpirciu 1:ceee5a608e7c 534 }
alexpirciu 1:ceee5a608e7c 535
alexpirciu 1:ceee5a608e7c 536 return getMedian();
alexpirciu 1:ceee5a608e7c 537 }
alexpirciu 1:ceee5a608e7c 538
alexpirciu 1:ceee5a608e7c 539 /* */
alexpirciu 1:ceee5a608e7c 540 /** @brief OperGet medianator
alexpirciu 1:ceee5a608e7c 541 *
alexpirciu 1:ceee5a608e7c 542 *
alexpirciu 1:ceee5a608e7c 543 * @return median value
alexpirciu 1:ceee5a608e7c 544 */
alexpirciu 1:ceee5a608e7c 545 template <class T, uint32_t N>
alexpirciu 1:ceee5a608e7c 546 T filter::nonlinear::siso::CMedianFilter<T, N>::getMedian()
alexpirciu 1:ceee5a608e7c 547 {
alexpirciu 1:ceee5a608e7c 548 T ret_val;
alexpirciu 1:ceee5a608e7c 549 if (1 == m_size % 2) // daca filtrul are lungime impara
alexpirciu 1:ceee5a608e7c 550 {
alexpirciu 1:ceee5a608e7c 551 ret_val = m_median->info;
alexpirciu 1:ceee5a608e7c 552 }
alexpirciu 1:ceee5a608e7c 553 else // daca filtrul are lungime para
alexpirciu 1:ceee5a608e7c 554 {
alexpirciu 1:ceee5a608e7c 555 ret_val = 0.5*(m_median->info + m_median->prev->info);
alexpirciu 1:ceee5a608e7c 556 }
alexpirciu 1:ceee5a608e7c 557 return ret_val;
alexpirciu 1:ceee5a608e7c 558 }