I found a solution. I was using the ellipse centre instead of the 2nd focus when calculating the distance equation. So, I calculate the 2nd focus now, and I use it.
struct EllipseParameters2 {
double semi_major_axis;
double eccentricity;
double angle;
Eigen::Vector2d center;
};
// Helper function for distance calculation
double distance(double x1, double y1, double x2, double y2)
{
return sqrt(pow(x2 - x1, 2) + pow(y2 - y1, 2));
}
// Objective function for optimization (least squares)
double objectiveFunction(
const VectorXd& params,
const vector<cartesian_point>& points,
const vector<cartesian_point>& velocities,
const cartesian_point& focus)
{
double h = params[0], k = params[1], a = params[2], b = params[3];
double error = 0;
EllipseParameters2 ep;
ep.angle = 0;
ep.center(0) = 0;
ep.center(1) = k;
ep.semi_major_axis = a;
ep.eccentricity = sqrt(1 - (b * b) / (a * a));
Eigen::Vector2d focus1;
focus1(0) = focus.x;
focus1(1) = focus.y;
Eigen::Vector2d focus2 = calculateSecondFocus(ep, focus1);
for(size_t i = 0; i < points.size(); i++)
{
cartesian_point p = points[i];
cartesian_point v = velocities[i];
double dist1 = distance(p.x, p.y, focus1(0), focus1(1));
double dist2 = distance(p.x, p.y, focus2(0), focus2(1));
error += pow(dist1 + dist2 - 2 * a, 2);
// Since we're axis-aligned, we simplify velocity condition:
// Velocity should be more in line with the axis of the ellipse
double velError = 0;
if (abs(v.x) > abs(v.y)) // Suggesting a is along x
velError = pow(v.y / v.x - (k - p.y) / (h - p.x), 2); // Check alignment with y
else
velError = pow(v.x / v.y - (h - p.x) / (k - p.y), 2); // Check alignment with x
error += velError;
}
return error;
}
Also, I tweaked how the gradient descent initial parameters are set.
// Simple solver function using gradient descent (for demonstration)
VectorXd solveEllipseParameters(const vector<cartesian_point>& points, const vector<cartesian_point>& velocities, const cartesian_point& focus)
{
// Get max distance data
vector<double> mvec;
mvec.push_back(points[0].length());
mvec.push_back(points[1].length());
mvec.push_back(points[2].length());
mvec.push_back(points[3].length());
mvec.push_back(points[4].length());
sort(mvec.begin(), mvec.end());
// Use the maximum distance data
const double m = mvec[4];
const double d = (mvec[4] - mvec[0]) / mvec[4];
VectorXd params(4); // h, k, a, b
cout << "d: " << d << endl;
if(d < 0.1)
params << 1, 1, m, m; // Initial guess
else
params << 1, 1, m*0.125, m*0.125; // Initial guess
int iterations = 100000;
double stepSize = 0.0001;
for (int i = 0; i < iterations; i++)
{
VectorXd gradient = VectorXd::Zero(4);
for (int j = 0; j < 4; j++)
{
VectorXd paramsPlus = params;
paramsPlus[j] += stepSize;
VectorXd paramsMinus = params;
paramsMinus[j] -= stepSize;
gradient[j] = (objectiveFunction(paramsPlus, points, velocities, focus) - objectiveFunction(paramsMinus, points, velocities, focus)) / (2 * stepSize);
}
params -= stepSize * gradient;
}
return params;
}
The results are not quite perfect, but they're closer. Any suggestions on how to better go about calculating the initial parameters for the gradient descent are very welcome.