The problem to solve is finding the floating status of a floating body, given its weight and the center of gravity.
The function i use calculates the displaced volume and center of bouyance of the body given sinkage, heel and trim.
Where sinkage is a length unit and heel/trim is an angle limited to a value from -90 to 90.
The floating status is found when displaced volum is equal to weight and the center of gravity is in a vertical line with center of bouancy.
I have this implemeted as a non-linear Newton-Raphson root finding problem with 3 variables (sinkage, trim, heel) and 3 equations.
This method works, but needs good initial guesses. So I am hoping to find either a better approach for this, or a good method to find the initial values.
Below is the code for the newton and jacobian algorithm used for the Newton-Raphson iteration. The function volume takes the parameters sinkage, heel and trim. And returns volume, and the coordinates for center of bouyancy.
I also included the maxabs and GSolve2 algorithms, I belive these are taken from Numerical Recipies.
void jacobian(float x[], float weight, float vcg, float tcg, float lcg, float jac[][3], float f0[]) {
float h = 0.0001f;
float temp;
float j_volume, j_vcb, j_lcb, j_tcb;
float f1[3];
volume(x[0], x[1], x[2], j_volume, j_lcb, j_vcb, j_tcb);
f0[0] = j_volume-weight;
f0[1] = j_tcb-tcg;
f0[2] = j_lcb-lcg;
for (int i=0;i<3;i++) {
temp = x[i];
x[i] = temp + h;
volume(x[0], x[1], x[2], j_volume, j_lcb, j_vcb, j_tcb);
f1[0] = j_volume-weight;
f1[1] = j_tcb-tcg;
f1[2] = j_lcb-lcg;
x[i] = temp;
jac[0][i] = (f1[0]-f0[0])/h;
jac[1][i] = (f1[1]-f0[1])/h;
jac[2][i] = (f1[2]-f0[2])/h;
}
}
void newton(float weight, float vcg, float tcg, float lcg, float &sinkage, float &heel, float &trim) {
float x[3] = {10,1,1};
float accuracy = 0.000001f;
int ntryes = 30;
int i = 0;
float jac[3][3];
float max;
float f0[3];
float gauss_f0[3];
while (i < ntryes) {
jacobian(x, weight, vcg, tcg, lcg, jac, f0);
if (sqrt((f0[0]*f0[0]+f0[1]*f0[1]+f0[2]*f0[2])/2) < accuracy) {
break;
}
gauss_f0[0] = -f0[0];
gauss_f0[1] = -f0[1];
gauss_f0[2] = -f0[2];
GSolve2(jac, 3, gauss_f0);
x[0] = x[0]+gauss_f0[0];
x[1] = x[1]+gauss_f0[1];
x[2] = x[2]+gauss_f0[2];
// absmax(x) - Return absolute max value from an array
max = absmax(x);
if (max < 1) max = 1;
if (sqrt((gauss_f0[0]*gauss_f0[0]+gauss_f0[1]*gauss_f0[1]+gauss_f0[2]*gauss_f0[2])) < accuracy*max) {
x[0]=x2[0];
x[1]=x2[1];
x[2]=x2[2];
break;
}
i++;
}
sinkage = x[0];
heel = x[1];
trim = x[2];
}
int GSolve2(float a[][3],int n,float b[]) {
float x,sum,max,temp;
int i,j,k,p,m,pos;
int nn = n-1;
for (k=0;k<=n-1;k++)
{
/* pivot*/
max=fabs(a[k][k]);
pos=k;
for (p=k;p<n;p++){
if (max < fabs(a[p][k])){
max=fabs(a[p][k]);
pos=p;
}
}
if (ABS(a[k][pos]) < EPS) {
writeLog("Matrix is singular");
break;
}
if (pos != k) {
for(m=k;m<n;m++){
temp=a[pos][m];
a[pos][m]=a[k][m];
a[k][m]=temp;
}
}
/* convert to upper triangular form */
if ( fabs(a[k][k])>=1.e-6)
{
for (i=k+1;i<n;i++)
{
x = a[i][k]/a[k][k];
for (j=k+1;j<n;j++) a[i][j] = a[i][j] -a[k][j]*x;
b[i] = b[i] - b[k]*x;
}
}
else
{
writeLog("zero pivot found in line:%d",k);
return 0;
}
}
/* back substitution */
b[nn] = b[nn] / a[nn][nn];
for (i=n-2;i>=0;i--)
{
sum = b[i];
for (j=i+1;j<n;j++)
sum = sum - a[i][j]*b[j];
b[i] = sum/a[i][i];
}
return 0;
}
float absmax(float x[]) {
int i = 1;
int n = sizeof(x);
float max = x[0];
while (i < n) {
if (max < x[i]) {
max = x[i];
}
i++;
}
return max;
}
Have you considered some stochastic search methods to find the initial value and then fine-tuning with Newton Raphson? One possibility is evolutionary computation, you can use the Inspyred package. For a physical problem similar in many ways to the one you describe, look at this example: http://inspyred.github.com/tutorial.html#lunar-explorer
What about using a damped version of Newton's method? You could quite easily modify your implementation to make it. Think about Newton's method as finding a direction
d_k = f(x_k) / f'(x_k)
and updating the variable
x_k+1 = x_k - L_k d_k
In the usual Newton's method, L_k is always 1, but this might create overshoots or undershoots. So, let your method chose L_k. Suppose that your method usually overshoots. A possible strategy consists in taking the largest L_k in the set {1,1/2,1/4,1/8,... L_min} such that the condition
|f(x_k+1)| <= (1-L_k/2) |f(x_k)|
is satisfied (or L_min if none of the values satisfies this criteria).
With the same criteria, another possible strategy is to start with L_0=1 and if the criteria is not met, try with L_0/2 until it works (or until L_0 = L_min). Then for L_1, start with min(1, 2L_0) and do the same. Then start with L_2=min(1, 2L_1) and so on.
By the way: are you sure that your problem has a unique solution? I guess that the answer to this question depends on the shape of your object. If you have a rugby ball, there's one angle that you cannot fix. So if your shape is close to such an object, I would not be surprised that the problem is difficult to solve for that angle.
Related
I have a function to detect the peak of real-time data. The algorithm is mentioned in this thread. which looks like this:
std::vector<int> smoothedZScore(std::vector<float> input)
{
//lag 5 for the smoothing functions
int lag = 5;
//3.5 standard deviations for signal
float threshold = 3.5;
//between 0 and 1, where 1 is normal influence, 0.5 is half
float influence = .5;
if (input.size() <= lag + 2)
{
std::vector<int> emptyVec;
return emptyVec;
}
//Initialise variables
std::vector<int> signal(input.size(), 0.0);
std::vector<float> filteredY(input.size(), 0.0);
std::vector<float> avgFilter(input.size(), 0.0);
std::vector<float> stdFilter(input.size(), 0.0);
std::vector<float> subVecStart(input.begin(), input.begin() + lag);
double sum = std::accumulate(std::begin(subVecStart), std::end(subVecStart), 0.0);
double mean = sum / subVecStart.size();
double accum = 0.0;
std::for_each (std::begin(subVecStart), std::end(subVecStart), [&](const double d) {
accum += (d - mean) * (d - mean);
});
double stdev = sqrt(accum / (subVecStart.size()-1));
//avgFilter[lag] = mean(subVecStart);
avgFilter[lag] = mean;
//stdFilter[lag] = stdDev(subVecStart);
stdFilter[lag] = stdev;
for (size_t i = lag + 1; i < input.size(); i++)
{
if (std::abs(input[i] - avgFilter[i - 1]) > threshold * stdFilter[i - 1])
{
if (input[i] > avgFilter[i - 1])
{
signal[i] = 1; //# Positive signal
}
else
{
signal[i] = -1; //# Negative signal
}
//Make influence lower
filteredY[i] = influence* input[i] + (1 - influence) * filteredY[i - 1];
}
else
{
signal[i] = 0; //# No signal
filteredY[i] = input[i];
}
//Adjust the filters
std::vector<float> subVec(filteredY.begin() + i - lag, filteredY.begin() + i);
// avgFilter[i] = mean(subVec);
// stdFilter[i] = stdDev(subVec);
}
return signal;
}
In my code, I'm reading real-time 3 axis accelerometer values from IMU sensor and displaying it as a graph. I need to detect the peak of the signal using the above algorithm. I added the function to my code.
Let's say the realtime valuees are following:
double x = sample->acceleration_g[0];
double y = sample->acceleration_g[1];
double z = sample->acceleration_g[2];
How do I pass this value to the above function and detect the peak.
I tried calling this:
smoothedZScore(x)
but gives me an error:
settings.cpp:230:40: error: no matching function for call to 'smoothedZScore'
settings.cpp:92:18: note: candidate function not viable: no known conversion from 'double' to 'std::vector<float>' for 1st argument
EDIT
The algorithm needs a minimum of 7 samples to feed in. So I guess I may need to store my realtime data in a buffer.
But I've difficulty understanding how to store samples in a buffer and apply to the peak detection algorithm.
can you show me a possible solution to this?
You will need to rewrite the algorithm. Your problem isn't just a realtime problem, you also need a causal solution. The function you have is not causal.
Practically speaking, you will need a class, and that class will need to incrementally calculate the standard deviation.
I have been trying to find a way to draw a curved line/cubic bezier line using a custom function. However, all the examples and such found on the internet, differ a little from each other and usually produce different results, why? . None of the ones i have tried produce the same result as windows api PolyBezier which is what i need.
This is my current code for drawing cubic bezier lines:
double Factorial(int number)
{
double factorial = 1;
if (number > 1)
{
for (int count = 1; count <= number; count++) factorial = factorial * count;
}
return factorial;
}
double choose(double a, double b)
{
return Factorial(a) / (Factorial(b) * Factorial(a - b));
}
VOID MyPolyBezier(HDC hdc, PPOINT Pts, int Total)
{
float x, y;
MoveToEx(hdc, Pts[0].x, Pts[0].y, 0);
Total -= 1;
//for (float t = 0; t <= 1; t += (1./128.))
for (float t = 0; t <= 1; t += 0.0078125)
{
x = 0;
y = 0;
for (int I = 0; I <= Total; I++)
{
x += Pts[I].x * choose(Total, I) * pow(1 - t, Total - I) * pow(t, I);
y += Pts[I].y * choose(Total, I) * pow(1 - t, Total - I) * pow(t, I);
}
LineTo(hdc, x, y);
}
}
And here is the code for testing it.
POINT TestPts[4];
BYTE TestType[4] = {PT_MOVETO, PT_BEZIERTO, PT_BEZIERTO, PT_BEZIERTO};
//set x, y points for the curved line.
TestPts[0].x = 50;
TestPts[0].y = 200;
TestPts[1].x = 100;
TestPts[1].y = 100;
TestPts[2].x = 150;
TestPts[2].y = 200;
TestPts[3].x = 200;
TestPts[3].y = 200;
//Draw using custom function.
MyPolyBezier(hdc, TestPts, 4);
//Move the curve down some.
TestPts[0].y += 10;
TestPts[1].y += 10;
TestPts[2].y += 10;
TestPts[3].y += 10;
//Draw using windows api.
//PolyDraw(hdc, TestPts, TestType, 4); //PolyDraw gives the same result as PolyBezier.
PolyBezier(hdc, TestPts, 4);
And an attached image of my bad results:
Note: the bottom bezier line is windows(PolyBezier) version.
Edit:
the final goal, Windows(On the left) VS custom funtion. Hopefully this helps in some way.
So a cubic bezier is a mathematical curve. The cubic bezier is a specific case of a more general curve.
The cubic bezier is defined by 4 control points -- a start and end point, and 2 control points. In general, a bezier has n control points in order.
The line is drawn as a time parameter t goes from 0 to 1.
To find out where a general bezier of degree n is at time t:
For each adjacent pair of control points in your bezier, find the weighted average of them, as controlled by t. So at + b(1-t) for control points a before b.
Use these n-1 points to form a degree n-1 bezier.
Solve the new bezier at time t.
when you hit a degree 1 bezier, stop. That is your point.
Try writing an algorithm based off the true definition of bezier, and see where it differs from the windows curve. This may ne less frustrating than taking some approximation and having two sets of errors to reconcile.
I'm trying to implement a very simple 1-dimensional gradient descent algorithm. The code I have does not work at all. Basically depending on my alpha value, the end parameters will either be wildly huge (like ~70 digits), or basically zero (~ 0.000). I feel like a gradient descent should not be nearly this sensitive in alpha (I'm generating small data in [0.0,1.0], but I think the gradient itself should account for the scale of the data, no?).
Here's the code:
#include <cstdio>
#include <cstdlib>
#include <ctime>
#include <vector>
using namespace std;
double a, b;
double theta0 = 0.0, theta1 = 0.0;
double myrand() {
return double(rand()) / RAND_MAX;
}
double f(double x) {
double y = a * x + b;
y *= 0.1 * (myrand() - 0.5); // +/- 5% noise
return y;
}
double h(double x) {
return theta1 * x + theta0;
}
int main() {
srand(time(NULL));
a = myrand();
b = myrand();
printf("set parameters: a = %lf, b = %lf\n", a, b);
int N = 100;
vector<double> xs(N);
vector<double> ys(N);
for (int i = 0; i < N; ++i) {
xs[i] = myrand();
ys[i] = f(xs[i]);
}
double sensitivity = 0.008;
double d0, d1;
for (int n = 0; n < 100; ++n) {
d0 = d1 = 0.0;
for (int i = 0; i < N; ++i) {
d0 += h(xs[i]) - ys[i];
d1 += (h(xs[i]) - ys[i]) * xs[i];
}
theta0 -= sensitivity * d0;
theta1 -= sensitivity * d1;
printf("theta0: %lf, theta1: %lf\n", theta0, theta1);
}
return 0;
}
Changing the value of alpha can produce the algorithm to diverge, so that may be one of the causes of what is happening. You can check by computing the error in each iteration and see if is increasing or decreasing.
In adition, it is recommended to set randomly the values of theta at the beginning in stead of assigning them to zero.
Apart from that, you should divide by N when you update the value of theta as follows:
theta0 -= sensitivity * d0/N;
theta1 -= sensitivity * d1/N;
I had a quick look at your implementation and it looks fine to me.
The code I have does not work at all.
I wouldn't say that. It seems to behave correctly for small enough values of sensitivity, which is a value that you just have to "guess", and that is how the gradient descent is supposed to work.
I feel like a gradient descent should not be nearly this sensitive in alpha
If you struggle to visualize that, remember that you are using gradient descent to find the minimum of the cost function of linear regression, which is a quadratic function. If you plot the cost function you will see why the learning rate is so sensitive in these cases: intuitively, if the parabola is narrow, the algorithm will converge more quickly, which is good, but then the learning rate is more "sensitive" and the algorithm can easily diverge if you are not careful.
I've been working on this for several weeks but have been unable to get my algorithm working properly and i'm at my wits end. Here's an illustration of what i have achieved:
If everything was working i would expect a perfect circle/oval at the end.
My sample points (in white) are recalculated every time a new control point (in yellow) is added. At 4 control points everything looks perfect, again as i add a 5th on top of the 1st things look alright, but then on the 6th it starts to go off too the side and on the 7th it jumps up to the origin!
Below I'll post my code, where calculateWeightForPointI contains the actual algorithm. And for reference- here is the information i'm trying to follow. I'd be so greatful if someone could take a look for me.
void updateCurve(const std::vector<glm::vec3>& controls, std::vector<glm::vec3>& samples)
{
int subCurveOrder = 4; // = k = I want to break my curve into to cubics
// De boor 1st attempt
if(controls.size() >= subCurveOrder)
{
createKnotVector(subCurveOrder, controls.size());
samples.clear();
for(int steps=0; steps<=20; steps++)
{
// use steps to get a 0-1 range value for progression along the curve
// then get that value into the range [k-1, n+1]
// k-1 = subCurveOrder-1
// n+1 = always the number of total control points
float t = ( steps / 20.0f ) * ( controls.size() - (subCurveOrder-1) ) + subCurveOrder-1;
glm::vec3 newPoint(0,0,0);
for(int i=1; i <= controls.size(); i++)
{
float weightForControl = calculateWeightForPointI(i, subCurveOrder, controls.size(), t);
newPoint += weightForControl * controls.at(i-1);
}
samples.push_back(newPoint);
}
}
}
//i = the weight we're looking for, i should go from 1 to n+1, where n+1 is equal to the total number of control points.
//k = curve order = power/degree +1. eg, to break whole curve into cubics use a curve order of 4
//cps = number of total control points
//t = current step/interp value
float calculateWeightForPointI( int i, int k, int cps, float t )
{
//test if we've reached the bottom of the recursive call
if( k == 1 )
{
if( t >= knot(i) && t < knot(i+1) )
return 1;
else
return 0;
}
float numeratorA = ( t - knot(i) );
float denominatorA = ( knot(i + k-1) - knot(i) );
float numeratorB = ( knot(i + k) - t );
float denominatorB = ( knot(i + k) - knot(i + 1) );
float subweightA = 0;
float subweightB = 0;
if( denominatorA != 0 )
subweightA = numeratorA / denominatorA * calculateWeightForPointI(i, k-1, cps, t);
if( denominatorB != 0 )
subweightB = numeratorB / denominatorB * calculateWeightForPointI(i+1, k-1, cps, t);
return subweightA + subweightB;
}
//returns the knot value at the passed in index
//if i = 1 and we want Xi then we have to remember to index with i-1
float knot(int indexForKnot)
{
// When getting the index for the knot function i remember to subtract 1 from i because of the difference caused by us counting from i=1 to n+1 and indexing a vector from 0
return knotVector.at(indexForKnot-1);
}
//calculate the whole knot vector
void createKnotVector(int curveOrderK, int numControlPoints)
{
int knotSize = curveOrderK + numControlPoints;
for(int count = 0; count < knotSize; count++)
{
knotVector.push_back(count);
}
}
Your algorithm seems to work for any inputs I tried it on. Your problem might be a that a control point is not where it is supposed to be, or that they haven't been initialized properly. It looks like there are two control-points, half the height below the bottom left corner.
I have been working on a project that will, in short, generate a 2D matrix of numbers, with "empty" spaces are represented by 0's. Each number is connected by a list of nodes. The nodes contain the number value, the number's X and Y position, and a list of all spaces adjacent to it (its "neighbors"), with the exception of spaces diagonally adjacent to the point, due to the algorithm only allowing movements of up, down, left, and right. The issue that I am having is that, as the title would suggest, I am experiencing some stack overflow issues. I will post my code below, if anyone could help, I would be most appreciative.
CoordList* Puzzle::GeneratePath(CoordList* Path, int GoalX, int GoalY)
{
int CurrX;
int CurrY;
CurrX = Path->NeighborX;
CurrY = Path->NeighborY;
if(CurrX == GoalX && CurrY == GoalY)
{
return(Path);
}
else
{
int NewX;
int NewY;
double NewDistance;
int OldX;
int OldY;
double OldDistance;
CoordList* PointNeighbors = NULL;
CoordList* BestChoice = NULL;
for(int i = 0; i < NumDirections; i++)
{
CoordList* NewNeighbor = new CoordList;
NewX = CurrX + DirectsX[i];
NewY = CurrY + DirectsY[i];
if(IsPossible(NewX, NewY))
{
NewNeighbor->NeighborX = NewX;
NewNeighbor->NeighborY = NewY;
if(PointNeighbors == NULL)
{
NewNeighbor->next = NULL;
PointNeighbors = NewNeighbor;
}
else
{
NewNeighbor->next = PointNeighbors;
PointNeighbors = NewNeighbor;
}
}
//delete NewNeighbor;
}
while(PointNeighbors != NULL)
{
if(BestChoice == NULL)
{
CoordList* AChoice = new CoordList;
AChoice->next = NULL;
NewX = PointNeighbors->NeighborX;
NewY = PointNeighbors->NeighborY;
AChoice->NeighborX = NewX;
AChoice->NeighborY = NewY;
BestChoice = AChoice;
PointNeighbors = PointNeighbors->next;
//delete AChoice;
}
else
{
NewX = PointNeighbors->NeighborX;
NewY = PointNeighbors->NeighborY;
NewDistance = DetermineDistance(NewX, NewY, GoalX, GoalY);
OldX = BestChoice->NeighborX;
OldY = BestChoice->NeighborY;
OldDistance = DetermineDistance(OldX, OldY, GoalX, GoalY);
if(NewDistance < OldDistance)
{
BestChoice->NeighborX = NewX;
BestChoice->NeighborY = NewY;
}
PointNeighbors = PointNeighbors->next;
}
}
BestChoice->next = Path;
Path = BestChoice;
return(GeneratePath(Path, GoalX, GoalY));
}
}
I was asked to provide my determine distance function. This is just a simple implementation of the traditional Point Distance formula. Provided below.
double Puzzle::DetermineDistance(int OneX, int OneY, int TwoX, int TwoY)
{
int DifX;
int DifY;
double PointSum;
DifX = (TwoX - OneX);
DifY = (TwoY - OneY);
DifX = (DifX * DifX);
DifY = (DifY * DifY);
PointSum = (DifX + DifY);
return (sqrt(PointSum));
}
The following is the IsPossible function, which determines if an X and Y value lies within the possible grid space.
bool Puzzle::IsPossible(int x, int y)
{
if(x + 1 > Size - 1 || x - 1 < 0
|| y + 1 > Size - 1 || y - 1 < 0)
{
return false;
}
return true;
}
You might have a infinite recursion loop that causes the stackoverflow, as you make new local variables every recursion, especially with your observered oscillation behaviour. I assume you dont have that problem with small matrices. Its just a shot in the dark :-)
The oscillation problem indicates that you dont check whether you have already been on one place already?
Anyways, maybe you want to reconsider using another pathfinding algorithm. I would suggest a agent based solution. I used to use the following solution to solve a maze of similar structure: I started an agent with a "PositionsList" of spots where it have been, so in the beginning only with the starting point. Then it copied itself to every reachable position not being in his own PositionList, adding the new position to that list and destroying itself then. Repeat that pattern with all new agents until the first agent reaches the goal. That way you are guaranteed to find the optimal path. But it might get pretty memory heavy for big matrices, especially when there are a lot different ways to get to the goal and a lot of possible directions per position! But there are plenty of other very good pathfinding algorithms out there. Maybe one of them suits you well :-)
Good Luck!