search

cwrucutter / snowmower_localization

An EFK for the snowmower.
MIT License
1 stars 1 forks source link

Make sure we are not taking square root of a negative value when there is a decawve measurement of -1 (i.e. bad measurement). #31

Closed kleinma closed 8 years ago

kleinma commented 8 years ago

https://github.com/cwrucutter/snowmower_localization/blob/1c1ea38bc43369e5d111ad427bd2c815abcef81c/src/ekf.cpp#L192-L281

kleinma commented 8 years ago

In hDecaWave, all the square roots look protected from a negative value. Each is the sum of two squares.

double d1 = sqrt(pow(dw1x-xTag,2)+pow(dw1y-yTag,2));                          
double d2 = sqrt(pow(dw2x-xTag,2)+pow(dw2y-yTag,2));                          
double d3 = sqrt(pow(dw3x-xTag,2)+pow(dw3y-yTag,2));                          
double d4 = sqrt(pow(dw4x-xTag,2)+pow(dw4y-yTag,2));
kleinma commented 8 years ago

in HDecaWave I wrote the square roots slightly differently. But, they still seem to be protected in the same way, with a minimum number of 0 under the radical.

double dist1 = pow(pow(dw1x-xTag,2)+pow(dw1y-yTag,2),0.5);
kleinma commented 8 years ago

I am currently under the impression that a divide by zero is causing the problem. I'm going to close this issue and open another.