[3] | 1 | #ifndef __GAUSSIAN_FILTERING_BASE__
|
---|
| 2 | #define __GAUSSIAN_FILTERING_BASE__
|
---|
| 3 |
|
---|
| 4 | #include "kalman_filtering.hpp"
|
---|
| 5 | #include "../math/pdf.hpp"
|
---|
| 6 |
|
---|
| 7 | #include <vector>
|
---|
| 8 |
|
---|
| 9 | namespace filter{
|
---|
| 10 |
|
---|
| 11 | namespace gaussiansum{
|
---|
| 12 |
|
---|
| 13 | using namespace math;
|
---|
| 14 | // using namespace rng;
|
---|
| 15 | using namespace ublas;
|
---|
| 16 |
|
---|
| 17 | /*!
|
---|
| 18 | * \class Gaussian
|
---|
| 19 | * \brief This class describes a basic weighted kalman state
|
---|
| 20 | */
|
---|
| 21 | class Gaussian : public filter::kalman::State{
|
---|
| 22 | public :
|
---|
| 23 |
|
---|
| 24 | /*!
|
---|
| 25 | * \brief Constructor
|
---|
| 26 | * \param state_size : Size of the state vector
|
---|
| 27 | * \param weight_ : Initial weight
|
---|
| 28 | */
|
---|
| 29 | Gaussian(const size_t &state_size, const double &weight_){
|
---|
| 30 | filter::kalman::State::Allocator(state_size);
|
---|
| 31 | weight=weight_;
|
---|
| 32 | }
|
---|
| 33 |
|
---|
| 34 | /**
|
---|
| 35 | * \brief Constructor
|
---|
| 36 | * \param state_size : Size of the state vector
|
---|
| 37 | */
|
---|
| 38 |
|
---|
| 39 | Gaussian(const size_t &state_size){
|
---|
| 40 | filter::kalman::State::Allocator(state_size);
|
---|
| 41 | weight=0;
|
---|
| 42 | }
|
---|
| 43 |
|
---|
| 44 | double weight; /*!< Weight of the Kalman state */
|
---|
| 45 |
|
---|
| 46 | /*
|
---|
| 47 | protected :*/
|
---|
| 48 |
|
---|
| 49 | // Gaussian:weight(1){};
|
---|
| 50 | };
|
---|
| 51 |
|
---|
| 52 |
|
---|
| 53 | /*!
|
---|
| 54 | * \class GaussianSet
|
---|
| 55 | * \brief This method describes a set of gaussians \n
|
---|
| 56 | * A set of gaussains is reprented by a vector of weighted kalman state \n
|
---|
| 57 | * somes methods can be applied to the set of particles like :\n
|
---|
| 58 | * estimate computation, resampling scheme or normalization method \n
|
---|
| 59 | */
|
---|
| 60 | template<class G=Gaussian> class GaussianSet {
|
---|
| 61 | public :
|
---|
| 62 |
|
---|
| 63 | /** Normalize the weights of gaussian states*/
|
---|
| 64 | void NormalizeWeights();
|
---|
| 65 |
|
---|
| 66 | /*!
|
---|
| 67 | * \brief Allocate the set of gaussians
|
---|
| 68 | * \param ngaussian : number of gaussians in the set
|
---|
| 69 | */
|
---|
| 70 | void Allocator(const size_t &ngaussian);
|
---|
| 71 |
|
---|
| 72 | /*!
|
---|
| 73 | * \brief Compute the estimate
|
---|
| 74 | * \return Vector
|
---|
| 75 | */
|
---|
| 76 | Vector Estimate();
|
---|
| 77 |
|
---|
| 78 | /*!
|
---|
| 79 | * \brief Destructor
|
---|
| 80 | */
|
---|
| 81 | ~GaussianSet(){}
|
---|
| 82 |
|
---|
| 83 |
|
---|
| 84 | std::vector<G> gaussians; /*!< gaussian states */
|
---|
| 85 |
|
---|
| 86 |
|
---|
| 87 | };
|
---|
| 88 |
|
---|
| 89 |
|
---|
| 90 | template<class G> void GaussianSet<G>::NormalizeWeights(){
|
---|
| 91 | double sum=0;
|
---|
| 92 | for(typename std::vector<G>::iterator I=gaussians.begin();I!=gaussians.end();I++)
|
---|
| 93 | sum+=(*I).weight;
|
---|
| 94 |
|
---|
| 95 | if(sum==0){throw filter_error("Gaussian set normalization weight sum =0");}
|
---|
| 96 | for(typename std::vector<G>::iterator I=gaussians.begin();I!=gaussians.end();I++)
|
---|
| 97 | (*I).weight/=sum;
|
---|
| 98 | }
|
---|
| 99 |
|
---|
| 100 |
|
---|
| 101 | template<class G> void GaussianSet<G>::Allocator(const size_t &ngaussian){
|
---|
| 102 | gaussians.reserve(ngaussian);
|
---|
| 103 | }
|
---|
| 104 |
|
---|
| 105 | template<class G> Vector GaussianSet<G>::Estimate(){
|
---|
| 106 | Vector estimate = ZeroVector(gaussians[0].X.size());
|
---|
| 107 | for(typename std::vector<G>::iterator I=gaussians.begin();I!=gaussians.end();I++)
|
---|
| 108 | estimate+=(*I).X*(*I).weight;
|
---|
| 109 | return estimate;
|
---|
| 110 | }
|
---|
| 111 |
|
---|
| 112 |
|
---|
| 113 | /*!
|
---|
| 114 | * \class LinearDynamicEquation
|
---|
| 115 | * \brief This class describes a linear dynamic equation
|
---|
| 116 | */
|
---|
| 117 | template <template <class> class S=GaussianSet, class G=Gaussian > class LinearDynamicEquation:public filter::kalman::LinearDynamicEquation<G>{
|
---|
| 118 | public :
|
---|
| 119 | /*!
|
---|
| 120 | * \brief virtual method where parameters of the dynamic equation must be evaluated
|
---|
| 121 | * \param s : weighted kalman state at time k-1
|
---|
| 122 | */
|
---|
| 123 | virtual void EvaluateParameters(G *s)=0;
|
---|
| 124 |
|
---|
| 125 | /*!
|
---|
| 126 | * \brief virtual method where the a posteriori state vector must be computed
|
---|
| 127 | * \param in : the a posteriori set of gaussian at time k-1
|
---|
| 128 | * \param out : the a priori set of gaussian at time k
|
---|
| 129 | */
|
---|
| 130 | virtual void Predict(S<G> *in,S<G> *out);
|
---|
| 131 |
|
---|
| 132 | /*!
|
---|
| 133 | * \brief Destructor
|
---|
| 134 | */
|
---|
| 135 | virtual ~LinearDynamicEquation(){}
|
---|
| 136 | };
|
---|
| 137 |
|
---|
| 138 |
|
---|
| 139 | template <template <class> class S, class G> void LinearDynamicEquation<S,G>::Predict(S<G> *in,S<G> *out){
|
---|
| 140 | for(size_t i=0;i<in->gaussians.size();i++){
|
---|
| 141 | EvaluateParameters(&in->gaussians[i]);
|
---|
| 142 |
|
---|
| 143 | out->gaussians[i].P= filter::kalman::LinearDynamicEquation<G>::_A*in->gaussians[i].P*Trans( filter::kalman::LinearDynamicEquation<G>::_A)
|
---|
| 144 | +filter::kalman::LinearDynamicEquation<G>::_B*filter::kalman::LinearDynamicEquation<G>::_Q*Trans(filter::kalman::LinearDynamicEquation<G>::_B);
|
---|
| 145 | out->gaussians[i].X= filter::kalman::LinearDynamicEquation<G>::_A*in->gaussians[i].X
|
---|
| 146 | +filter::kalman::LinearDynamicEquation<G>::_B*filter::kalman::LinearDynamicEquation<G>::_U;
|
---|
| 147 | out->gaussians[i].weight=in->gaussians[i].weight;
|
---|
| 148 | }
|
---|
| 149 | }
|
---|
| 150 |
|
---|
| 151 |
|
---|
| 152 | /*!
|
---|
| 153 | * \class NonLinearDynamicEquation
|
---|
| 154 | * \brief This class describes a non linear dynamic equation
|
---|
| 155 | */
|
---|
| 156 | template <template <class> class S=GaussianSet, class G=Gaussian > class NonLinearDynamicEquation:public filter::kalman::NonLinearDynamicEquation<G>{
|
---|
| 157 | public :
|
---|
| 158 | /*!
|
---|
| 159 | * \brief virtual method where parameters of the dynamic equation must be evaluated
|
---|
| 160 | * \param s : weighted kalman state at time k-1
|
---|
| 161 | * f= \n
|
---|
| 162 | * F= \n
|
---|
| 163 | * G= \n
|
---|
| 164 | */
|
---|
| 165 | virtual void EvaluateParameters(G *s)=0;
|
---|
| 166 |
|
---|
| 167 | /*!
|
---|
| 168 | * \brief virtual method where the a posteriori state vector must be computed
|
---|
| 169 | * \param in : the a posteriori set of gaussian at time k-1
|
---|
| 170 | * \param out : the a priori set of gaussian at time k
|
---|
| 171 | */
|
---|
| 172 | virtual void Predict(S<G> *in,S<G> *out);
|
---|
| 173 |
|
---|
| 174 | /*!
|
---|
| 175 | * \brief Destructor
|
---|
| 176 | */
|
---|
| 177 | virtual ~NonLinearDynamicEquation(){}
|
---|
| 178 | };
|
---|
| 179 |
|
---|
| 180 |
|
---|
| 181 | template <template <class> class S, class G> void NonLinearDynamicEquation<S,G>::Predict(S<G> *in,S<G> *out){
|
---|
| 182 | for(size_t i=0;i<in->gaussians.size();i++){
|
---|
| 183 | EvaluateParameters(&in->gaussians[i]);
|
---|
| 184 |
|
---|
| 185 | out->gaussians[i].P=filter::kalman::NonLinearDynamicEquation<G>::_F*in->gaussians[i].P*Trans(filter::kalman::NonLinearDynamicEquation<G>::_F)
|
---|
| 186 | +filter::kalman::NonLinearDynamicEquation<G>::_G*filter::kalman::NonLinearDynamicEquation<G>::_Q*Trans(filter::kalman::NonLinearDynamicEquation<G>::_G);
|
---|
| 187 | out->gaussians[i].X=filter::kalman::NonLinearDynamicEquation<G>::_f;
|
---|
| 188 | out->gaussians[i].weight=in->gaussians[i].weight;
|
---|
| 189 | }
|
---|
| 190 | }
|
---|
| 191 |
|
---|
| 192 | /*!
|
---|
| 193 | * \class LinearMeasureEquation
|
---|
| 194 | * \brief This class describes a linear measure equation
|
---|
| 195 | */
|
---|
| 196 | template <template <class> class S=GaussianSet, class G=Gau
|
---|
| 197 | ssian >class LinearMeasureEquation:public filter::kalman::LinearMeasureEquation<G>{
|
---|
| 198 | public :
|
---|
| 199 |
|
---|
| 200 | /*!
|
---|
| 201 | * \brief virtual method where parameters of the measure equation must be evaluated
|
---|
| 202 | * \param s : weighted kalman state at time k-1
|
---|
| 203 | */
|
---|
| 204 | virtual void EvaluateParameters(G *s)=0;
|
---|
| 205 |
|
---|
| 206 |
|
---|
| 207 | /*!
|
---|
| 208 | * \brief virtual method where the a posteriori state vector must be computed
|
---|
| 209 | * \param in : the a priori set of gaussian at time k
|
---|
| 210 | * \param out : the a posteriori set of gaussian at time k
|
---|
| 211 | */
|
---|
| 212 | virtual void Update(S<G> *in,S<G> *out);
|
---|
| 213 |
|
---|
| 214 |
|
---|
| 215 |
|
---|
| 216 |
|
---|
| 217 | /*!
|
---|
| 218 | * \brief Destructor
|
---|
| 219 | */
|
---|
| 220 | virtual ~LinearMeasureEquation(){};
|
---|
| 221 |
|
---|
| 222 | protected :
|
---|
| 223 |
|
---|
| 224 | /*!
|
---|
| 225 | * \brief virtual method where likelihood value for each particle is computed
|
---|
| 226 | * \param s : a priori weighted kalman state at time k
|
---|
| 227 | * \return likelihood value
|
---|
| 228 | */
|
---|
| 229 | virtual double ZPDF(G *s);
|
---|
| 230 |
|
---|
| 231 | };
|
---|
| 232 |
|
---|
| 233 |
|
---|
| 234 | template <template <class> class S, class G> void LinearMeasureEquation<S,G>::Update(S<G> *in,S<G> *out){
|
---|
| 235 |
|
---|
| 236 | for(size_t i=0;i<in->gaussians.size();i++){
|
---|
| 237 | filter::kalman::LinearMeasureEquation<G>::_coherency=true;
|
---|
| 238 | EvaluateParameters(&in->gaussians[i]);
|
---|
| 239 |
|
---|
| 240 | if(filter::kalman::LinearMeasureEquation<G>::_coherency){
|
---|
| 241 | filter::kalman::LinearMeasureEquation<G>::_K=in->gaussians[i].P*Trans(filter::kalman::LinearMeasureEquation<G>::_H) *
|
---|
| 242 | ( InvQR(filter::kalman::LinearMeasureEquation<G>::_H*in->gaussians[i].P*Trans(filter::kalman::LinearMeasureEquation<G>::_H)+filter::kalman::LinearMeasureEquation<G>::_R) );
|
---|
| 243 | out->gaussians[i].X=in->gaussians[i].X+filter::kalman::LinearMeasureEquation<G>::_K*(filter::kalman::LinearMeasureEquation<G>::_Z-filter::kalman::LinearMeasureEquation<G>::_H*in->gaussians[i].X);
|
---|
| 244 | out->gaussians[i].P=in->gaussians[i].P-filter::kalman::LinearMeasureEquation<G>::_K*filter::kalman::LinearMeasureEquation<G>::_H*in->gaussians[i].P;
|
---|
| 245 | }
|
---|
| 246 |
|
---|
| 247 | out->gaussians[i].weigh=ZPDF(&in->gaussian[i])*in->gaussians[i].weight;
|
---|
| 248 |
|
---|
| 249 | }
|
---|
| 250 |
|
---|
| 251 | //out->NormalizeWeights();
|
---|
| 252 | }
|
---|
| 253 |
|
---|
| 254 | template <template <class> class S, class G> double LinearMeasureEquation<S,G>::ZPDF(G *s){
|
---|
| 255 | Vector apvec=filter::kalman::LinearMeasureEquation<G>::_H*s->X;
|
---|
| 256 | Matrix apcov=filter::kalman::LinearMeasureEquation<G>::_H*s->P*Trans(filter::kalman::LinearMeasureEquation<G>::_H)+filter::kalman::LinearMeasureEquation<G>::_R;
|
---|
| 257 | Matrix apinv=InvQR(apcov);
|
---|
| 258 | double apdet=Det(apcov);
|
---|
| 259 | return (std::exp(-0.5*Dot(filter::kalman::LinearMeasureEquation<G>::_Z-apvec,apinv*(filter::kalman::LinearMeasureEquation<G>::_Z-apvec)))/
|
---|
| 260 | (std::sqrt(std::pow(2*M_PI,static_cast<int>(apvec.size()))*std::abs(apdet))));
|
---|
| 261 | }
|
---|
| 262 |
|
---|
| 263 |
|
---|
| 264 | /*!
|
---|
| 265 | * \class NonLinearMeasureEquation
|
---|
| 266 | * \brief This class describes a non linear measure equation
|
---|
| 267 | */
|
---|
| 268 | template <template <class> class S=GaussianSet, class G=Gaussian > class NonLinearMeasureEquation:public filter::kalman::NonLinearMeasureEquation<G>{
|
---|
| 269 | public :
|
---|
| 270 | /*!
|
---|
| 271 | * \brief virtual method where parameters of the measure equation must be evaluated
|
---|
| 272 | * \param s : weighted kalman state at time k-1
|
---|
| 273 | * h= \n
|
---|
| 274 | * H= \n
|
---|
| 275 | */
|
---|
| 276 | virtual void EvaluateParameters(G *s )=0;
|
---|
| 277 |
|
---|
| 278 | /*!
|
---|
| 279 | * \brief virtual method where the a posteriori state vector must be computed
|
---|
| 280 | * \param in : the a priori set of gaussian at time k
|
---|
| 281 | * \param out : the a posteriori set of gaussian at time k
|
---|
| 282 | */
|
---|
| 283 | virtual void Update(S<G> *in,S<G> *out);
|
---|
| 284 |
|
---|
| 285 | /*!
|
---|
| 286 | * \brief destructor
|
---|
| 287 | */
|
---|
| 288 | virtual ~NonLinearMeasureEquation(){}
|
---|
| 289 |
|
---|
| 290 | protected :
|
---|
| 291 |
|
---|
| 292 | /*!
|
---|
| 293 | * \brief virtual method where likelihood value for each particle is computed
|
---|
| 294 | * \param s : a priori weighted kalman state at time k
|
---|
| 295 | * \return likelihood value
|
---|
| 296 | */
|
---|
| 297 | virtual double ZPDF(G *s);
|
---|
| 298 |
|
---|
| 299 | };
|
---|
| 300 |
|
---|
| 301 | template <template <class> class S, class G> void NonLinearMeasureEquation<S,G>::Update(S<G> *in,S<G> *out){
|
---|
| 302 |
|
---|
| 303 | for(size_t i=0;i<in->gaussians.size();i++){
|
---|
| 304 | filter::kalman::NonLinearMeasureEquation<G>::_coherency=true;
|
---|
| 305 | EvaluateParameters(&in->gaussians[i]);
|
---|
| 306 |
|
---|
| 307 | if(filter::kalman::NonLinearMeasureEquation<G>::_coherency){
|
---|
| 308 | filter::kalman::NonLinearMeasureEquation<G>::_K=in->gaussians[i].P*Trans(filter::kalman::NonLinearMeasureEquation<G>::_H)*(InvQR(filter::kalman::NonLinearMeasureEquation<G>::_H*in->gaussians[i].P*Trans(filter::kalman::NonLinearMeasureEquation<G>::_H) + filter::kalman::NonLinearMeasureEquation<G>::_R));
|
---|
| 309 | out->gaussians[i].X=in->gaussians[i].X+filter::kalman::NonLinearMeasureEquation<G>::_K*(filter::kalman::NonLinearMeasureEquation<G>::_Z-filter::kalman::NonLinearMeasureEquation<G>::_h);
|
---|
| 310 | out->gaussians[i].P=in->gaussians[i].P-filter::kalman::NonLinearMeasureEquation<G>::_K*filter::kalman::NonLinearMeasureEquation<G>::_H*in->gaussians[i].P;
|
---|
| 311 | }
|
---|
| 312 | out->gaussians[i].weight=ZPDF(&in->gaussians[i])*in->gaussians[i].weight;
|
---|
| 313 | }
|
---|
| 314 |
|
---|
| 315 | //out->NormalizeWeights();
|
---|
| 316 | }
|
---|
| 317 |
|
---|
| 318 | template <template <class> class S, class G> double NonLinearMeasureEquation<S,G>::ZPDF(G *s){
|
---|
| 319 | Vector apvec=filter::kalman::NonLinearMeasureEquation<G>::_H*s->X;
|
---|
| 320 | Matrix apcov=filter::kalman::NonLinearMeasureEquation<G>::_H*s->P*Trans(filter::kalman::NonLinearMeasureEquation<G>::_H)+filter::kalman::NonLinearMeasureEquation<G>::_R;
|
---|
| 321 | Matrix apinv=InvQR(apcov);
|
---|
| 322 | double apdet=Det(apcov);
|
---|
| 323 | return (std::exp(-0.5*Dot(filter::kalman::NonLinearMeasureEquation<G>::_Z-apvec,apinv*(filter::kalman::NonLinearMeasureEquation<G>::_Z-apvec)))/
|
---|
| 324 | (std::sqrt(std::pow(2*M_PI,static_cast<int>(apvec.size()))*std::abs(apdet))));
|
---|
| 325 | }
|
---|
| 326 |
|
---|
| 327 | };
|
---|
| 328 | };
|
---|
| 329 |
|
---|
| 330 | #endif
|
---|