worked on grid-walking
worked on grid-generation added helper library for nav-meshes started working on nav meshes
This commit is contained in:
@@ -5,7 +5,7 @@
|
||||
#include "../../../misc/Time.h"
|
||||
#include "../../../math/Interpolator.h"
|
||||
|
||||
#include"../../../sensors/pressure/ActivityButterPressure.h"
|
||||
#include"../../../sensors/activity/ActivityButterPressure.h"
|
||||
|
||||
#include <KLib/misc/gnuplot/Gnuplot.h>
|
||||
#include <KLib/misc/gnuplot/GnuplotPlot.h>
|
||||
@@ -23,234 +23,234 @@
|
||||
|
||||
TEST(Butterworth, offlineSinus) {
|
||||
|
||||
//input data
|
||||
std::minstd_rand gen;
|
||||
std::uniform_real_distribution<double> noise (-0.1, +0.1);
|
||||
//input data
|
||||
std::minstd_rand gen;
|
||||
std::uniform_real_distribution<double> noise (-0.1, +0.1);
|
||||
|
||||
int size = 1100; //Fs
|
||||
double* input = new double[size];
|
||||
double* output = new double[size];
|
||||
int size = 1100; //Fs
|
||||
double* input = new double[size];
|
||||
double* output = new double[size];
|
||||
|
||||
// 17.5hz sin signal with random noise [-0.1, 0.1]
|
||||
for( int i=0; i < size; ++i ){
|
||||
input[i] = sin(0.1 * i) + noise(gen);
|
||||
}
|
||||
// 17.5hz sin signal with random noise [-0.1, 0.1]
|
||||
for( int i=0; i < size; ++i ){
|
||||
input[i] = sin(0.1 * i) + noise(gen);
|
||||
}
|
||||
|
||||
//butterworth
|
||||
Filter::ButterworthLP<double> butter(size,20,5);
|
||||
butter.stepInitialization(0);
|
||||
butter.filter(input, output, size, 0, true);
|
||||
//butterworth
|
||||
Filter::ButterworthLP<double> butter(size,20,5);
|
||||
butter.stepInitialization(0);
|
||||
butter.filter(input, output, size, 0, true);
|
||||
|
||||
K::Gnuplot gp;
|
||||
K::GnuplotPlot plot;
|
||||
K::GnuplotPlotElementLines linesInput;
|
||||
K::GnuplotPlotElementLines linesOutput;
|
||||
K::Gnuplot gp;
|
||||
K::GnuplotPlot plot;
|
||||
K::GnuplotPlotElementLines linesInput;
|
||||
K::GnuplotPlotElementLines linesOutput;
|
||||
|
||||
for(int i=0; i < size-1; ++i){
|
||||
for(int i=0; i < size-1; ++i){
|
||||
|
||||
K::GnuplotPoint2 input_p1(i, input[i]);
|
||||
K::GnuplotPoint2 input_p2(i+1, input[i+1]);
|
||||
K::GnuplotPoint2 input_p1(i, input[i]);
|
||||
K::GnuplotPoint2 input_p2(i+1, input[i+1]);
|
||||
|
||||
K::GnuplotPoint2 output_p1(i, output[i]);
|
||||
K::GnuplotPoint2 output_p2(i+1, output[i+1]);
|
||||
K::GnuplotPoint2 output_p1(i, output[i]);
|
||||
K::GnuplotPoint2 output_p2(i+1, output[i+1]);
|
||||
|
||||
linesInput.addSegment(input_p1, input_p2);
|
||||
linesOutput.addSegment(output_p1, output_p2);
|
||||
}
|
||||
linesInput.addSegment(input_p1, input_p2);
|
||||
linesOutput.addSegment(output_p1, output_p2);
|
||||
}
|
||||
linesOutput.getStroke().getColor().setHexStr("#00FF00");
|
||||
|
||||
plot.add(&linesInput);
|
||||
plot.add(&linesOutput);
|
||||
plot.add(&linesInput);
|
||||
plot.add(&linesOutput);
|
||||
|
||||
gp.draw(plot);
|
||||
gp.flush();
|
||||
gp.draw(plot);
|
||||
gp.flush();
|
||||
|
||||
sleep(10);
|
||||
sleep(10);
|
||||
}
|
||||
|
||||
TEST(Butterworth, onlineSinus) {
|
||||
|
||||
int size = 1100; //Fs
|
||||
double* input = new double[size];
|
||||
double* output = new double[size];
|
||||
int size = 1100; //Fs
|
||||
double* input = new double[size];
|
||||
double* output = new double[size];
|
||||
|
||||
Filter::ButterworthLP<double> butter(size,20,5);
|
||||
butter.stepInitialization(0);
|
||||
Filter::ButterworthLP<double> butter(size,20,5);
|
||||
butter.stepInitialization(0);
|
||||
|
||||
//input data
|
||||
std::minstd_rand gen;
|
||||
std::uniform_real_distribution<double> noise (-0.1, +0.1);
|
||||
//input data
|
||||
std::minstd_rand gen;
|
||||
std::uniform_real_distribution<double> noise (-0.1, +0.1);
|
||||
|
||||
// 17.5hz sin signal with random noise [-0.1, 0.1]
|
||||
for( int i=0; i < size; ++i ){
|
||||
input[i] = sin(0.1 * i) + noise(gen);
|
||||
// 17.5hz sin signal with random noise [-0.1, 0.1]
|
||||
for( int i=0; i < size; ++i ){
|
||||
input[i] = sin(0.1 * i) + noise(gen);
|
||||
|
||||
output[i] = butter.process(input[i]);
|
||||
}
|
||||
output[i] = butter.process(input[i]);
|
||||
}
|
||||
|
||||
K::Gnuplot gp;
|
||||
K::GnuplotPlot plot;
|
||||
K::GnuplotPlotElementLines linesInput;
|
||||
K::GnuplotPlotElementLines linesOutput;
|
||||
K::Gnuplot gp;
|
||||
K::GnuplotPlot plot;
|
||||
K::GnuplotPlotElementLines linesInput;
|
||||
K::GnuplotPlotElementLines linesOutput;
|
||||
|
||||
for(int i=0; i < size-1; ++i){
|
||||
for(int i=0; i < size-1; ++i){
|
||||
|
||||
K::GnuplotPoint2 input_p1(i, input[i]);
|
||||
K::GnuplotPoint2 input_p2(i+1, input[i+1]);
|
||||
K::GnuplotPoint2 input_p1(i, input[i]);
|
||||
K::GnuplotPoint2 input_p2(i+1, input[i+1]);
|
||||
|
||||
K::GnuplotPoint2 output_p1(i, output[i]);
|
||||
K::GnuplotPoint2 output_p2(i+1, output[i+1]);
|
||||
K::GnuplotPoint2 output_p1(i, output[i]);
|
||||
K::GnuplotPoint2 output_p2(i+1, output[i+1]);
|
||||
|
||||
linesInput.addSegment(input_p1, input_p2);
|
||||
linesOutput.addSegment(output_p1, output_p2);
|
||||
}
|
||||
linesInput.addSegment(input_p1, input_p2);
|
||||
linesOutput.addSegment(output_p1, output_p2);
|
||||
}
|
||||
linesOutput.getStroke().getColor().setHexStr("#00FF00");
|
||||
|
||||
plot.add(&linesInput);
|
||||
plot.add(&linesOutput);
|
||||
plot.add(&linesInput);
|
||||
plot.add(&linesOutput);
|
||||
|
||||
gp.draw(plot);
|
||||
gp.flush();
|
||||
gp.draw(plot);
|
||||
gp.flush();
|
||||
|
||||
sleep(1);
|
||||
sleep(1);
|
||||
}
|
||||
|
||||
TEST(Butterworth, offlineOctaveBaro) {
|
||||
|
||||
|
||||
double* input = new double[100000];
|
||||
double* output = new double[100000];
|
||||
double* input = new double[100000];
|
||||
double* output = new double[100000];
|
||||
|
||||
Interpolator<int, double> interp;
|
||||
Interpolator<int, double> interp;
|
||||
|
||||
//read file
|
||||
std::string line;
|
||||
std::string filename = getDataFile("baro/logfile_UAH_R1_S4_baro.dat");
|
||||
std::ifstream infile(filename);
|
||||
//read file
|
||||
std::string line;
|
||||
std::string filename = getDataFile("baro/logfile_UAH_R1_S4_baro.dat");
|
||||
std::ifstream infile(filename);
|
||||
|
||||
int counter = 0;
|
||||
while (std::getline(infile, line))
|
||||
{
|
||||
std::istringstream iss(line);
|
||||
int ts;
|
||||
double value;
|
||||
int counter = 0;
|
||||
while (std::getline(infile, line))
|
||||
{
|
||||
std::istringstream iss(line);
|
||||
int ts;
|
||||
double value;
|
||||
|
||||
while (iss >> ts >> value) {
|
||||
while (iss >> ts >> value) {
|
||||
|
||||
interp.add(ts, value);
|
||||
interp.add(ts, value);
|
||||
|
||||
while(interp.getMaxKey() > counter*20 ){
|
||||
double interpValue = interp.get(counter*20);
|
||||
while(interp.getMaxKey() > counter*20 ){
|
||||
double interpValue = interp.get(counter*20);
|
||||
|
||||
input[counter] = interpValue;
|
||||
//std::cout << counter*20 << " " << interpValue << " i" << std::endl;
|
||||
++counter;
|
||||
}
|
||||
input[counter] = interpValue;
|
||||
//std::cout << counter*20 << " " << interpValue << " i" << std::endl;
|
||||
++counter;
|
||||
}
|
||||
|
||||
//std::cout << ts << " " << value << " r" << std::endl;
|
||||
//std::cout << ts << " " << value << " r" << std::endl;
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
Filter::ButterworthLP<double> butter(50,0.2,2);
|
||||
butter.filter(input, output, counter, 938.15, true);
|
||||
Filter::ButterworthLP<double> butter(50,0.2,2);
|
||||
butter.filter(input, output, counter, 938.15, true);
|
||||
|
||||
K::Gnuplot gp;
|
||||
K::GnuplotPlot plot;
|
||||
K::GnuplotPlotElementLines linesInput;
|
||||
K::GnuplotPlotElementLines linesOutput;
|
||||
K::Gnuplot gp;
|
||||
K::GnuplotPlot plot;
|
||||
K::GnuplotPlotElementLines linesInput;
|
||||
K::GnuplotPlotElementLines linesOutput;
|
||||
|
||||
for(int i=0; i < counter-1; ++i){
|
||||
for(int i=0; i < counter-1; ++i){
|
||||
|
||||
K::GnuplotPoint2 input_p1(i, input[i]);
|
||||
K::GnuplotPoint2 input_p2(i+1, input[i+1]);
|
||||
K::GnuplotPoint2 input_p1(i, input[i]);
|
||||
K::GnuplotPoint2 input_p2(i+1, input[i+1]);
|
||||
|
||||
K::GnuplotPoint2 output_p1(i, output[i]);
|
||||
K::GnuplotPoint2 output_p2(i+1, output[i+1]);
|
||||
K::GnuplotPoint2 output_p1(i, output[i]);
|
||||
K::GnuplotPoint2 output_p2(i+1, output[i+1]);
|
||||
|
||||
linesInput.addSegment(input_p1, input_p2);
|
||||
linesOutput.addSegment(output_p1, output_p2);
|
||||
}
|
||||
linesInput.addSegment(input_p1, input_p2);
|
||||
linesOutput.addSegment(output_p1, output_p2);
|
||||
}
|
||||
linesOutput.getStroke().getColor().setHexStr("#00FF00");
|
||||
|
||||
plot.add(&linesInput);
|
||||
plot.add(&linesOutput);
|
||||
plot.add(&linesInput);
|
||||
plot.add(&linesOutput);
|
||||
|
||||
gp.draw(plot);
|
||||
gp.flush();
|
||||
gp.draw(plot);
|
||||
gp.flush();
|
||||
|
||||
sleep(1);
|
||||
sleep(1);
|
||||
|
||||
}
|
||||
|
||||
TEST(Butterworth, onlineOctaveBaro) {
|
||||
|
||||
std::vector<double> input;
|
||||
std::vector<double> output;
|
||||
std::vector<double> input;
|
||||
std::vector<double> output;
|
||||
|
||||
Interpolator<int, double> interp;
|
||||
Interpolator<int, double> interp;
|
||||
|
||||
Filter::ButterworthLP<double> butter(50,0.02,2);
|
||||
butter.stepInitialization(938.15);
|
||||
Filter::ButterworthLP<double> butter(50,0.02,2);
|
||||
butter.stepInitialization(938.15);
|
||||
|
||||
//read file
|
||||
std::string line;
|
||||
std::string filename = getDataFile("baro/logfile_UAH_R1_S4_baro.dat");
|
||||
std::ifstream infile(filename);
|
||||
//read file
|
||||
std::string line;
|
||||
std::string filename = getDataFile("baro/logfile_UAH_R1_S4_baro.dat");
|
||||
std::ifstream infile(filename);
|
||||
|
||||
int counter = 1;
|
||||
while (std::getline(infile, line))
|
||||
{
|
||||
std::istringstream iss(line);
|
||||
int ts;
|
||||
double value;
|
||||
int counter = 1;
|
||||
while (std::getline(infile, line))
|
||||
{
|
||||
std::istringstream iss(line);
|
||||
int ts;
|
||||
double value;
|
||||
|
||||
while (iss >> ts >> value) {
|
||||
while (iss >> ts >> value) {
|
||||
|
||||
interp.add(ts, value);
|
||||
interp.add(ts, value);
|
||||
|
||||
while(interp.getMaxKey() > counter*20 ){
|
||||
double interpValue = interp.get(counter*20);
|
||||
while(interp.getMaxKey() > counter*20 ){
|
||||
double interpValue = interp.get(counter*20);
|
||||
|
||||
//std::cout << counter*20 << " " << interpValue << " i" << std::endl;
|
||||
//std::cout << counter*20 << " " << interpValue << " i" << std::endl;
|
||||
|
||||
input.push_back(interpValue);
|
||||
input.push_back(interpValue);
|
||||
|
||||
output.push_back(butter.process(interpValue));
|
||||
output.push_back(butter.process(interpValue));
|
||||
|
||||
++counter;
|
||||
}
|
||||
++counter;
|
||||
}
|
||||
|
||||
//std::cout << ts << " " << value << " r" << std::endl;
|
||||
//std::cout << ts << " " << value << " r" << std::endl;
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
K::Gnuplot gp;
|
||||
K::GnuplotPlot plot;
|
||||
K::GnuplotPlotElementLines linesInput;
|
||||
K::GnuplotPlotElementLines linesOutput;
|
||||
K::Gnuplot gp;
|
||||
K::GnuplotPlot plot;
|
||||
K::GnuplotPlotElementLines linesInput;
|
||||
K::GnuplotPlotElementLines linesOutput;
|
||||
|
||||
for(int i=0; i < input.size()-1; ++i){
|
||||
for(int i=0; i < input.size()-1; ++i){
|
||||
|
||||
K::GnuplotPoint2 input_p1(i, input[i]);
|
||||
K::GnuplotPoint2 input_p2(i+1, input[i+1]);
|
||||
K::GnuplotPoint2 input_p1(i, input[i]);
|
||||
K::GnuplotPoint2 input_p2(i+1, input[i+1]);
|
||||
|
||||
K::GnuplotPoint2 output_p1(i, output[i]);
|
||||
K::GnuplotPoint2 output_p2(i+1, output[i+1]);
|
||||
K::GnuplotPoint2 output_p1(i, output[i]);
|
||||
K::GnuplotPoint2 output_p2(i+1, output[i+1]);
|
||||
|
||||
linesInput.addSegment(input_p1, input_p2);
|
||||
linesOutput.addSegment(output_p1, output_p2);
|
||||
}
|
||||
linesInput.addSegment(input_p1, input_p2);
|
||||
linesOutput.addSegment(output_p1, output_p2);
|
||||
}
|
||||
linesOutput.getStroke().getColor().setHexStr("#00FF00");
|
||||
|
||||
plot.add(&linesInput);
|
||||
plot.add(&linesOutput);
|
||||
plot.add(&linesInput);
|
||||
plot.add(&linesOutput);
|
||||
|
||||
gp.draw(plot);
|
||||
gp.flush();
|
||||
gp.draw(plot);
|
||||
gp.flush();
|
||||
|
||||
sleep(1);
|
||||
sleep(1);
|
||||
}
|
||||
|
||||
|
||||
|
||||
Reference in New Issue
Block a user