Optimization problem for frenet-serret fit

Question

Let's suppose we have a trajectory $\mathbf{X} = \left[x \; \;y \right]^{T}$. Frenet-Serret model for 2D would consist of following equations: $$ \dot{\mathbf{X}}_{model} = V(t){\mathbf{T}} $$ $$ \dot{\mathbf{T}} = V(t)\kappa(t){\mathbf{N}} $$ $$ \dot{\mathbf{N}} = -V(t)\kappa(t){\mathbf{T}} $$

My goal is to find $V(t)$ and $\kappa(t)$ for which $\left||\mathbf{X}_{model}-\mathbf{X}\right||^{2}$ is minimum. Here are my issues:

1. What kind of function is generic enough to depict the shape of an unknown curve? I am using orthogonal polynomials for now and optimization problem solve for their coefficients. I am getting very poor results.
2. Velocity needs to be positive for these equations. This I can only do through non linear constraint but I do not want to include that to an already complicated optimization problem. I would like to choose a function which is generic enough but is positive definite for all of dependent variable. What function can do this?
3. Is this problem even solvable? As soon as 2nd iteration, curvature and velocity values go in the range where equations are almost non-differentiable. Is there a way to fix that? Ideally I would like to add lower and upper bounds on the co-efficients but I do not know if that is the right approach.

Here is how I have approached this problem so far.

1. Assume some shape of $V(t)$ and $\kappa(t)$. Constants of these polynomials are unknown variables we are trying to find by minimizing the objective function $\left||\mathbf{X}_{model}-\mathbf{X}\right||^{2}$.
2. We also have to consider $\mathbf{T}$ at time step $0$. This create one more optimization variable (In 2D we can create a unit vector from an angle, thus we solve for an angle and calculate the unit vector). $\mathbf{N}$ by definition is perpendicular to $\mathbf{T}$ so we do not have to solve for it.
3. Use equations list above to compute $\mathbf{X}_{model}$.
4. Calculate least square error as stated above.

I am provided snippets of my code just to give an idea how problem is formalized. I am using fmincon from MATLAB. Thus step 1 to 4 goes into a objective function file as follows:

function obj = minimize_func_timedep_rk_planer(xdata,shape_data,theta_1,Curvature_coeff,Velocity_coeff)
 
T0 = s2c(theta_1) ; % T vector
N0 = [-T0(2) T0(1)] ; % N vector

x0 = [T0;N0] ; 
tim = 1:size(xdata,2);
Curvature = Cur_time(tim,Curvature_coeff,shape_data.Curvature) ; % Creates Curvature array based on the assumed shape
Velocity =  Vel_time(tim,Velocity_coeff,shape_data.Velocity) ;% Creates velocity array based on the assumed shape

h = 1e-2 ; % time step range kutta 4th order
[t_com, ~] = rng_kutta_model_planer(Curvature,Velocity,tim,h,x0) ; % an implementation of 4th order range kutta model. 

x_model = compute_xposition_planer(Velocity,t_com,xdata(:,1)); 
obj = sum(vecnorm(xdata-x_model).^2) ;
end

I cannot use matlab ODE functions because $\mathbf{T}$ and $\mathbf{N}$ vectors need to be normalized after each time step.

Code for fmincon is as follows:

A = [];
b = [];
Aeq = [];
Beq = [];
lb = [0,-inf(1,length([A_cur A_vel)])] ;
ub = [2*pi,inf(1,length([A_cur A_vel)])] ;
initial_cond_minimization = [t_1,A_cur,A_vel] ; % These initial conditions are computed from known curve
options = optimoptions('fmincon','Display','iter','Algorithm','interior-point');
fitfcn_variable_parameters = @(f) minimize_func_timedep_rk_planer(xdata,shape_data,f(1), ...
        f(2: 2+curve_Terms-1), ...
        f(2+curve_Terms: ...
                     2+curve_Terms + Vel_Terms-1)) ;

 F = fmincon(fitfcn_variable_parameters,initial_cond_minimization,A,b,Aeq,Beq,lb,ub,[],options);