FHWS/IPIN2017
Archived
3
0
Fork 0
Code Issues 4 Pull Requests Releases Wiki Activity

added the mixing particle filter model with all is might and failures :)

Browse Source
This commit is contained in:
toni
2017-04-16 01:11:59 +02:00
parent c7691a81f0
commit 1f6df67010
6 changed files with 239 additions and 74 deletions
Download Patch File Download Diff File Expand all files Collapse all files

107
code/main.cpp
View File

@@ -9,6 +9,7 @@
#include <sys/types.h>
#include <sys/stat.h>
#include <KLib/math/filter/merging/InteractingMultipleModelParticleFilter.h>
//frank
//const std::string mapDir = "/mnt/data/workspaces/IPIN2016/IPIN2016/competition/maps/";
@@ -162,32 +163,41 @@ void run(DataSetup setup, int numFile, std::string folder, std::vector<int> gtPa
ctrl.resetAfterTransition();
MyObs obs;
//random start position
std::unique_ptr<K::ParticleFilterInitializer<MyState>> init(new PFInit(grid)); std::move(init);
//filter init
//std::unique_ptr<PFInit> init =
K::ParticleFilterHistory<MyState, MyControl, MyObs> pf(Settings::numParticles, std::unique_ptr<PFInit>(new PFInit(grid)));
//K::ParticleFilterHistory<MyState, MyControl, MyObs> pf(Settings::numParticles, std::unique_ptr<PFInitFixed>(new PFInitFixed(grid, GridPoint(1120.0f, 750.0f, 740.0f), 90.0f)));
pf.setTransition(std::unique_ptr<PFTrans>(new PFTrans(grid, &ctrl)));
pf.setEvaluation(std::unique_ptr<PFEval>(new PFEval(WiFiModel, beaconModel, grid)));
//init the mode filters
std::vector<K::ParticleFilterMixing<MyState, MyControl, MyObs>> modes;
//resampling
if(Settings::useKLB){
pf.setResampling(std::unique_ptr<K::ParticleFilterResamplingDivergence<MyState>>(new K::ParticleFilterResamplingDivergence<MyState>()));
} else {
//pf.setResampling(std::unique_ptr<K::ParticleFilterResamplingSimple<MyState>>(new K::ParticleFilterResamplingSimple<MyState>()));
pf.setResampling(std::unique_ptr<K::ParticleFilterResamplingPercent<MyState>>(new K::ParticleFilterResamplingPercent<MyState>(0.4)));
//pf.setResampling(std::unique_ptr<NodeResampling<MyState, MyNode>>(new NodeResampling<MyState, MyNode>(*grid)););
}
std::shared_ptr<K::ParticleFilterInitializer<MyState>> init(new PFInit(grid));
//std::shared_ptr<K::ParticleFilterInitializer<MyState>> init(new PFInitFixed(grid, GridPoint(1120.0f, 750.0f, 740.0f), 90.0f));
pf.setNEffThreshold(0.95);
// mode 1
K::ParticleFilterMixing<MyState, MyControl, MyObs> mode1(Settings::numParticles, init, Settings::Mode1::modeProbability);
mode1.setTransition(std::shared_ptr<PFTrans>(new PFTrans(grid, &ctrl)));
mode1.setEvaluation(std::shared_ptr<PFEval>(new PFEval(WiFiModel, beaconModel, grid)));
mode1.setResampling(std::shared_ptr<K::ParticleFilterResamplingSimple<MyState>>(new K::ParticleFilterResamplingSimple<MyState>()));
mode1.setNEffThreshold(0.95);
mode1.setEstimator(std::shared_ptr<K::ParticleFilterEstimationWeightedAverage<MyState>>(new K::ParticleFilterEstimationWeightedAverage<MyState>()));
//estimation
//pf.setEstimation(std::unique_ptr<K::ParticleFilterEstimationWeightedAverage<MyState>>(new K::ParticleFilterEstimationWeightedAverage<MyState>()));
//pf.setEstimation(std::unique_ptr<K::ParticleFilterEstimationRegionalWeightedAverage<MyState>>(new K::ParticleFilterEstimationRegionalWeightedAverage<MyState>()));
pf.setEstimation(std::unique_ptr<K::ParticleFilterEstimationOrderedWeightedAverage<MyState>>(new K::ParticleFilterEstimationOrderedWeightedAverage<MyState>(0.5)));
//pf.setEstimation(std::unique_ptr<K::ParticleFilterEstimationKernelDensity<MyState, 3>>(new K::ParticleFilterEstimationKernelDensity<MyState, 3>()));
modes.push_back(mode1);
// mode 2
K::ParticleFilterMixing<MyState, MyControl, MyObs> mode2(Settings::numParticles, init, Settings::Mode2::modeProbability);
mode2.setTransition(std::shared_ptr<PFTransSimple>(new PFTransSimple(grid)));
mode2.setEvaluation(std::shared_ptr<PFEval>(new PFEval(WiFiModel, beaconModel, grid)));
mode2.setResampling(std::shared_ptr<K::ParticleFilterResamplingSimple<MyState>>(new K::ParticleFilterResamplingSimple<MyState>()));
mode2.setNEffThreshold(0.95);
mode2.setEstimator(std::shared_ptr<K::ParticleFilterEstimationWeightedAverage<MyState>>(new K::ParticleFilterEstimationWeightedAverage<MyState>()));
modes.push_back(mode2);
//init mixing
Eigen::MatrixXd transitionProbabilityMatrix(2,2);
transitionProbabilityMatrix << 1,0,0,1;
K::InteractingMultipleModelParticleFilter<MyState, MyControl, MyObs> IMMAPF(modes, transitionProbabilityMatrix);
IMMAPF.setMixingSampler(std::unique_ptr<K::MixingSamplerDivergency<MyState, MyControl, MyObs>>(new K::MixingSamplerDivergency<MyState, MyControl, MyObs>()));
IMMAPF.setJointEstimation(std::unique_ptr<K::JointEstimationPosteriorOnly<MyState, MyControl, MyObs>>(new K::JointEstimationPosteriorOnly<MyState, MyControl, MyObs>()));
IMMAPF.setMarkovTransitionProbability(std::unique_ptr<ModeProbabilityTransition>(new ModeProbabilityTransition(grid, Settings::Mixing::lambda)));
Timestamp lastTimestamp = Timestamp::fromMS(0);
@@ -201,9 +211,6 @@ void run(DataSetup setup, int numFile, std::string folder, std::vector<int> gtPa
K::Statistics<float> errorStats;
//calc wi-fi prob for every node and get mean vector
WiFiObserverFree wiFiProbability(Settings::WiFiModel::sigma, WiFiModel);
//file writing for error data
const long int t = static_cast<long int>(time(NULL));
@@ -256,7 +263,6 @@ void run(DataSetup setup, int numFile, std::string folder, std::vector<int> gtPa
} else if (e.type == Offline::Sensor::GRAVITY) {
md.addGravity(ts, fr.getGravity()[e.idx].data);
Eigen::Vector2f curVec = md.getCurrentMotionAxis();
ctrl.motionDeltaAngle_rad = md.getMotionChangeInRad();
}
@@ -265,34 +271,14 @@ void run(DataSetup setup, int numFile, std::string folder, std::vector<int> gtPa
obs.currentTime = ts;
MyState est;
if(Settings::useKLB){
const WiFiMeasurements wifiObs = Settings::WiFiModel::vg_eval.group(obs.wifi);
//update filter
est = IMMAPF.update(&ctrl, obs);
std::vector<MyNode> allNodes = grid.getNodes();
std::vector<K::Particle<MyState>> particleWifi;
for(MyNode node : allNodes){
double prob = wiFiProbability.getProbability(node, ts, wifiObs);
K::Particle<MyState> tmp (MyState(GridPoint(node.x_cm, node.y_cm, node.z_cm)), prob);
particleWifi.push_back(tmp);
}
if(kld_data.empty()){
kld_data.push_back(0.0);
}
double kld = 0.0;
//set probability distributions.
//std::function<double(std::vector<K::Particle<MyState>>&, MyState, std::vector<K::Particle<MyState>>&)> kldFunc = getKernelDensityProbability;
std::function<double(std::vector<K::Particle<MyState>>&, MyState, std::vector<K::Particle<MyState>>&)> kldFunc = kldFromMultivariatNormal;
//update filter
est = pf.update(&ctrl, obs, particleWifi, kldFunc, kld);
kld_data.push_back(kld);
} else {
est = pf.update(&ctrl, obs);
if(kld_data.empty()){
kld_data.push_back(0.0);
} else{
kld_data.push_back(__KLD);
}
Point3 estPos = est.position.inMeter();
@@ -314,7 +300,8 @@ void run(DataSetup setup, int numFile, std::string folder, std::vector<int> gtPa
plot.setEst(estPos);
plot.setGT(gtPos);
plot.addEstimationNode(estPos);
plot.addParticles(pf.getParticles());
plot.addParticles(IMMAPF.getModes()[0].getParticles());
plot.addEstimationNodeSmoothed(IMMAPF.getModes()[1].getEstimation().position.inMeter());
//plot.gp << "set arrow 919 from " << tt.pos.x << "," << tt.pos.y << "," << tt.pos.z << " to "<< tt.pos.x << "," << tt.pos.y << "," << tt.pos.z+1 << "lw 3\n";
@@ -404,13 +391,13 @@ int main(int argc, char** argv) {
//run(data.BERKWERK, 6, "EVALBERGWERK"); // Nexus vor
//for(int i = 0; i < 5; ++i){
Settings::useKLB = false;
//run(data.IPIN2017, 0, "ipin2017", Settings::Paths_IPIN2017::path1);
run(data.IPIN2017, 1, "ipin2017", Settings::Paths_IPIN2017::path1);
run(data.IPIN2017, 2, "ipin2017", Settings::Paths_IPIN2017::path2);
//run(data.IPIN2017, 3, "ipin2017", Settings::Paths_IPIN2017::path2);
run(data.IPIN2017, 5, "ipin2017", Settings::Paths_IPIN2017::path3);
//run(data.IPIN2017, 4, "ipin2017", Settings::Paths_IPIN2017::path3);
// Settings::useKLB = false;
// //run(data.IPIN2017, 0, "ipin2017", Settings::Paths_IPIN2017::path1);
// run(data.IPIN2017, 1, "ipin2017", Settings::Paths_IPIN2017::path1);
// run(data.IPIN2017, 2, "ipin2017", Settings::Paths_IPIN2017::path2);
// //run(data.IPIN2017, 3, "ipin2017", Settings::Paths_IPIN2017::path2);
// run(data.IPIN2017, 5, "ipin2017", Settings::Paths_IPIN2017::path3);
// //run(data.IPIN2017, 4, "ipin2017", Settings::Paths_IPIN2017::path3);
Settings::useKLB = true;
//run(data.IPIN2017, 0, "ipin2017", Settings::Paths_IPIN2017::path1);