void ParticleFilter::RunIteration(int odomTicks, double lat, double lon, double heading, double deltaTime) {
if(odomTicks == lastTick) return; // Do nothing if we have not moved
PropogateParticles(odomTicks, lat, lon, addAngle(heading, -startHeading));
ResampleParticles();
// Calculated Average
double x_total = 0;
double y_total = 0;
double tx_total = 0;
double ty_total = 0;
for(unsigned int i = 0; i < ParticleList.size(); i++) {
x_total += ParticleList[i].X;
y_total += ParticleList[i].Y;
tx_total += cos(ParticleList[i].Heading * D2R);
ty_total += sin(ParticleList[i].Heading * D2R);
}
tx_total /= ParticleList.size();
ty_total /= ParticleList.size();
this->currX = x_total / ParticleList.size();
this->currY = y_total / ParticleList.size();
this->currHeading = addAngle(R2D * atan2(ty_total, tx_total), 0);
}
void Gradient::createSegment(const point<int>& l,
const point<int>& r)
{
double radAngle = atan2((l.x - r.x), (r.y - l.y));
double sn = sin(radAngle);
double cs = cos(radAngle);
double x0 = l.x;
double y0 = l.y;
float length = Utility::getLength(l,r);
float xDiff = static_cast<float>(r.x - l.x)/length;
float yDiff = static_cast<float>(r.y - l.y)/length;
for (double u = 0; u <= length; u+=1.){
int x = (int)round(x0 + u * xDiff);
int y = (int)round(y0 + u * yDiff);
if ( abs(x) < IMAGE_WIDTH/2 &&
abs(y) < IMAGE_HEIGHT/2){
addAngle(static_cast<uint8_t>(radAngle/M_PI * 128),
static_cast<int16_t>(x),
static_cast<int16_t>(y));
}
}
}
/*
* Converts a lat long pair to a cartesian point relative to the
* startLon, startLat. North is positive y, East is positive X.
*/
void ParticleFilter :: GPS2Point(double lat, double lon, double& x, double& y) {
double dist = acos( sin(startLat*D2R) * sin(lat*D2R) +
cos(startLat*D2R) * cos(lat*D2R) *
cos(lat*D2R - startLat*D2R) * EARTH_RADIUS );
double x_angle = sin(abs(startLat - lat) * D2R) * cos(lat * D2R);
double y_angle = cos(startLat * D2R) * sin(lat * D2R) -
sin(startLat * D2R) * cos(lat * D2R) *
cos(abs(startLon - lon) * D2R);
double angle = addAngle(atan2(y_angle, x_angle), -startHeading);
x = dist * cos(angle);
y = dist * sin(angle);
}
void ParticleFilter::PropogateParticles(int odomTicks, double lat, double lon, double heading) {
int deltaTick = odomTicks - lastTick;
lastTick = odomTicks;
// Check for encoder rollover
if( deltaTick < 0) {
deltaTick += odomModel.maxTicks;
}
double distTravelled = deltaTick * odomModel.wheelCircum / odomModel.ticksPerRev;
double angleTraveled = heading - currHeading;
//std::cout << "\t distTravelled: " << distTravelled << std::endl;
//std::cout << "\t angleTraveled: " << angleTraveled << std::endl;
// Propogate each particle
highestWeight = 0;
for(unsigned int i = 0; i < ParticleList.size(); i++) {
// Add randomness
double trans = distTravelled - sample_normal_dist(0.05 * abs(distTravelled));
double rot = angleTraveled - sample_normal_dist(0.05 * abs(angleTraveled));
PropogatedList[i].X = ParticleList[i].X + trans * cos( D2R * (ParticleList[i].Heading + rot) );
PropogatedList[i].Y = ParticleList[i].Y + trans * sin( D2R * (ParticleList[i].Heading + rot) );
PropogatedList[i].Heading = addAngle(ParticleList[i].Heading, rot);
/*
if( i == 0) {
std::cout << "\t trans: " << trans << std::endl;
std::cout << "\t rot: " << rot << std::endl;
std::cout << "\t PropogatedList[i].X " << PropogatedList[i].X << std::endl;
std::cout << "\t PropogatedList[i].Y: " << PropogatedList[i].Y << std::endl;
std::cout << "\t PropogatedList[i].Heading: " << PropogatedList[i].Heading << std::endl;
}
*/
//highestWeight = 0;
WeightParticle (lat, lon, PropogatedList[i] );
if(highestWeight < PropogatedList[i].Weight)
highestWeight = PropogatedList[i].Weight;
}
}
void Gradient::createLineAtPoint(uint8_t angle, float radius)
{
float radAngle = static_cast<float>(angle) * M_PI_FLOAT/128.f;
double sn = sin(radAngle);
double cs = cos(radAngle);
double x0 = radius * cs;
double y0 = radius * sn;
for (double u = -200.; u <= 200.; u+=1.){
int x = (int)round(x0 + u * sn);
int y = (int)round(y0 - u * cs);
if ( abs(x) < IMAGE_WIDTH/2 &&
abs(y) < IMAGE_HEIGHT/2){
addAngle(angle,
static_cast<int16_t>(x),
static_cast<int16_t>(y));
}
}
}
void ConstrainAngleToolHelper::addAnglesDeg(double base, double stepping)
{
for (double angle = base; angle < base + 360; angle += stepping)
addAngle(angle * M_PI / 180);
}
void ConstrainAngleToolHelper::addAngles(double base, double stepping)
{
for (double angle = base; angle < base + 2*M_PI; angle += stepping)
addAngle(angle);
}
int main(int argc, char** argv) {
if (argc != 2) {
printHelp();
return EXIT_FAILURE;
}
SndfileHandle inputFile;
inputFile = SndfileHandle(argv[1]);
int framesCount = inputFile.frames();
int *buffer;
buffer = new int[framesCount];
inputFile.read(buffer, inputFile.frames());
#ifdef DEBUG
cerr<<"Samples size: "<<framesCount<<endl;
#endif
double firstAngle;
double deltaAngle;
string decoded = "";
/* finding angle of a sin between two samples in the input signal */
firstAngle = asin(((double)buffer[0]) / AMPLITUDE);
deltaAngle = asin(((double)buffer[1]) / AMPLITUDE) - firstAngle;
#ifdef DEBUG
cerr<<"deltaAngle: "<<deltaAngle<<" = "<<deltaAngle*180/PI<<"°"<<endl;
#endif
double actAngle = firstAngle;
double deltaX1 = deltaAngle;
double deltaX4 = deltaAngle;
/* QPSK bauds mapping values */
double expectedAngle1 = addAngle(0, 3*PI/4.0);
double expectedAngle2 = addAngle(0, PI/4.0);
double expectedAngle3 = addAngle(0, 5*PI/4.0);
double expectedAngle4 = addAngle(0, 7*PI/4.0);
int changes = 0;
int firstChange = 0;
int secondChange = 0;
int thirdChange = 0;
double expectedAngle1b = addNormalize(0, expectedAngle1);
double expectedAngle4b = addNormalize(0, expectedAngle4);
double *expectedAngle = &expectedAngle1b;
/* while 3 changes in sync part of the signal signal */
while (changes < 3) {
#ifdef DEBUG
cerr<<"loop: "<<thirdChange<<endl;
cerr<<"actAngle: "<<actAngle<<" = "<<actAngle*180/PI<<"° "<<sin(actAngle)<<endl;
cerr<<"expected: "<<*expectedAngle<<" = "<<*expectedAngle*180/PI<<"° "<<sin(*expectedAngle)<<endl;
#endif
/* we have a change in a baud */
if (!((actAngle > *expectedAngle - (PI/20))&&(actAngle < *expectedAngle + (PI/20)))) {
if (changes == 0) {
expectedAngle = &expectedAngle4b;
} else if (changes == 1) {
expectedAngle = &expectedAngle1b;
}
changes++;
}
/* counting samples till first change */
if (changes < 1) {
firstChange++;
}
/* counting samples till second change */
if (changes < 2) {
secondChange++;
}
/* counting samples till third change */
if (changes < 3) {
thirdChange++;
}
/* thirdChange is also a counter for this loop */
actAngle = asin(((double)buffer[thirdChange]) / AMPLITUDE);
expectedAngle1b = addNormalizeChangeDelta(expectedAngle1b, &deltaX1);
expectedAngle4b = addNormalizeChangeDelta(expectedAngle4b, &deltaX4);
}
/* preparing values for voting */
secondChange /= 2;
thirdChange /=3;
int samplesPerBaud = 0;
/* voting for samles per baud... we should have at least 2 same
* values to claim it as a samples per baud */
if (firstChange == secondChange) {
samplesPerBaud = firstChange;
} else if (secondChange == thirdChange) {
samplesPerBaud = secondChange;
} else if (firstChange == thirdChange) {
samplesPerBaud = firstChange;
}
#ifdef DEBUG
cerr<<"SPB: "<<samplesPerBaud<< " 1st: "<<firstChange<<" 2nd: "<<secondChange<<" 3rd: "<<thirdChange<<endl;
#endif
double res1 = 0;
double res2 = 0;
double res3 = 0;
double res4 = 0;
/* Iteraes through the all samples. For each baud we are looking
* for the sin values that differs the least from actual baud */
for (int i = 0, s = samplesPerBaud; i < framesCount; i++, s--) {
/* counting differences for each sin */
res1 += fabs(buffer[i] - sin(expectedAngle1));
res2 += fabs(buffer[i] - sin(expectedAngle2));
res3 += fabs(buffer[i] - sin(expectedAngle3));
res4 += fabs(buffer[i] - sin(expectedAngle4));
/* we have processed a baud and we are looking for the
* least change */
if (s == 0 || i == framesCount-1) {
s = samplesPerBaud;
double res12;
double res34;
int resId12 = 0;
int resId34 = 0;
if (res1 < res2) {
res12 = res1;
resId12 = 1;
} else {
res12 = res2;
resId12 = 2;
}
if (res3 < res4) {
res34 = res3;
resId34 = 3;
} else {
res34 = res4;
resId34 = 4;
}
int resId;
if (res12 < res34) {
resId = resId12;
} else {
resId = resId34;
}
switch(resId) {
case 1: decoded += "00"; break;
case 2: decoded += "01"; break;
case 3: decoded += "10"; break;
case 4: decoded += "11"; break;
default: break;
}
res1 = 0;
res2 = 0;
res3 = 0;
res4 = 0;
}
expectedAngle1 = addAngle(expectedAngle1, -deltaAngle);
expectedAngle2 = addAngle(expectedAngle2, -deltaAngle);
expectedAngle3 = addAngle(expectedAngle3, -deltaAngle);
expectedAngle4 = addAngle(expectedAngle4, -deltaAngle);
}
#ifdef DEBUG
cerr<<decoded<<endl;
cerr<<decoded.substr(8);
#endif
/* writting to an output file */
string outName = string(argv[1]);
outName = outName.replace(outName.end()-3, outName.end(), "txt");
ofstream outFile(outName);
if (!outFile.is_open()) {
cerr<<"Can not open output file\n";
return EXIT_FAILURE;
}
outFile<<decoded.substr(8);
outFile.close();
delete [] buffer;
return EXIT_SUCCESS;
}
