147 lines
3.9 KiB
C++
147 lines
3.9 KiB
C++
#ifdef WITH_TESTS
|
|
|
|
#include "../../../../smc/merging/mixing/MixingSamplerDivergency.h"
|
|
#include "../../../../smc/filtering/ParticleFilterMixing.h"
|
|
|
|
#include "../../../Tests.h"
|
|
|
|
namespace K {
|
|
|
|
struct MyState {
|
|
double x;
|
|
double y;
|
|
MyState() : x(0), y(0) {;}
|
|
MyState(double x, double y) : x(x), y(y) {;}
|
|
MyState& operator += (const MyState& other) {
|
|
x += other.x;
|
|
y += other.y;
|
|
return *this;
|
|
}
|
|
MyState& operator /= (double d) {
|
|
x /= d;
|
|
y /= d;
|
|
return *this;
|
|
}
|
|
MyState operator * (double d) const {
|
|
return MyState(x*d, y*d);
|
|
}
|
|
MyState& operator = (const MyState& other) {
|
|
this->x = other.x;
|
|
this->y = other.y;
|
|
return *this;
|
|
}
|
|
double distance(const MyState& o) const {
|
|
return std::sqrt( (x-o.x)*(x-o.x) + (y-o.y)*(y-o.y) ) / 4.9;
|
|
}
|
|
bool belongsToRegion(const MyState& o) const {
|
|
return distance(o) <= 1.0;
|
|
}
|
|
|
|
};
|
|
|
|
struct MyControl {
|
|
|
|
};
|
|
|
|
struct MyObservation {
|
|
double x;
|
|
double y;
|
|
MyObservation() : x(0), y(0) {;}
|
|
void set(double x, double y) {this->x = x; this->y = y;}
|
|
};
|
|
|
|
class MyInitializer1 : public SMC::ParticleFilterInitializer<MyState> {
|
|
void initialize(std::vector<SMC::Particle<MyState>>& particles) override {
|
|
for (SMC::Particle<MyState>& p : particles) {
|
|
p.state.x = 0;
|
|
p.state.y = 0;
|
|
p.weight = 1.0 / particles.size();
|
|
}
|
|
}
|
|
};
|
|
|
|
class MyInitializer2 : public SMC::ParticleFilterInitializer<MyState> {
|
|
void initialize(std::vector<SMC::Particle<MyState>>& particles) override {
|
|
for (SMC::Particle<MyState>& p : particles) {
|
|
p.state.x = 1;
|
|
p.state.y = 1;
|
|
p.weight = 1.0 / particles.size();
|
|
}
|
|
}
|
|
};
|
|
|
|
|
|
TEST(Mixing, standard) {
|
|
|
|
//init particle filters
|
|
int numParticles = 1000;
|
|
Eigen::MatrixXd transition(2,2);
|
|
transition << 0.8, 0.2, 0.2, 0.8;
|
|
|
|
SMC::ParticleFilterMixing<MyState, MyControl, MyObservation> mode1(numParticles, std::unique_ptr<MyInitializer1>(new MyInitializer1()), 0.5);
|
|
SMC::ParticleFilterMixing<MyState, MyControl, MyObservation> mode2(numParticles, std::unique_ptr<MyInitializer2>(new MyInitializer2()), 0.5);
|
|
|
|
std::vector<SMC::ParticleFilterMixing<MyState, MyControl, MyObservation>> modes;
|
|
modes.push_back(mode1);
|
|
modes.push_back(mode2);
|
|
|
|
|
|
SMC::MixingSamplerDivergency<MyState, MyControl, MyObservation> mixer;
|
|
|
|
//run the mixing and check results
|
|
for(int t = 0; t < 100; ++t){
|
|
mixer.mixAndSample(modes, transition);
|
|
|
|
int cnt_zero = 0;
|
|
int cnt_ones = 0;
|
|
for(int i = 0; i < modes[0].getParticles().size(); ++i){
|
|
|
|
if(modes[0].getParticles()[i].state.x == 0){
|
|
cnt_zero++;
|
|
}else{
|
|
cnt_ones++;
|
|
}
|
|
}
|
|
|
|
std::cout << "MODE1 Zeros: " << (double) cnt_zero / numParticles<< std::endl;
|
|
std::cout << "MODE1 Ones: " << (double) cnt_ones/ numParticles << std::endl;
|
|
|
|
|
|
cnt_zero = 0;
|
|
cnt_ones = 0;
|
|
for(int i = 0; i < modes[0].getParticles().size(); ++i){
|
|
|
|
if(modes[1].getParticles()[i].state.x == 0){
|
|
cnt_zero++;
|
|
}else{
|
|
cnt_ones++;
|
|
}
|
|
}
|
|
|
|
std::cout << "MODE2 Zeros: " << (double) cnt_zero / numParticles<< std::endl;
|
|
std::cout << "MODE2 Ones: " << (double) cnt_ones/ numParticles << std::endl;
|
|
}
|
|
|
|
|
|
|
|
}
|
|
|
|
TEST(Mixing, differentParticleSize) {
|
|
|
|
}
|
|
|
|
|
|
TEST(Mixing, dynamicTransitionMatrix) {
|
|
|
|
}
|
|
|
|
TEST(Mixing, ThreeFilters) {
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
#endif
|