public void ControlStep(double dt)
{
psi = Math.PI / 2 - psi;
psi_s = Math.PI / 2 - psi_s;
if (psi > Math.PI)
{
psi = psi - 2 * Math.PI;
}
if (psi_s > Math.PI)
{
psi_s = psi_s - 2 * Math.PI;
}
double sint_s = Math.Sin(theta_s);
double cost_s = Math.Sin(theta_s);
// Rotation matrix from Inertial to Frenet
Matrix R = new Matrix(3, 3);
R[0, 0] = Math.Cos(theta_s) * Math.Cos(psi_s);
R[0, 1] = Math.Cos(theta_s) * Math.Sin(psi_s);
R[0, 2] = -Math.Sin(theta_s);
R[1, 0] = -Math.Sin(psi_s);
R[1, 1] = Math.Cos(psi_s);
R[1, 2] = 0;
R[2, 0] = Math.Sin(theta_s) * Math.Cos(psi_s);
R[2, 1] = Math.Sin(theta_s) * Math.Sin(psi_s);
R[2, 2] = Math.Cos(theta_s);
Matrix p = new Matrix(3, 1);
p[0, 0] = pPoint.X;
p[1, 0] = pPoint.Y;
p[2, 0] = pPoint.Z;
Matrix q = new Matrix(3, 1);
q[0, 0] = qPoint.X;
q[1, 0] = qPoint.Y;
q[2, 0] = qPoint.Z;
Matrix d = R * (p - q);
// Calculate position error and angular error
ex = d[0, 0];
ey = d[1, 0];
ez = d[2, 0];
// calculate angular error e_psi = psi-psi_s;
double psi_tilde = psi - psi_s;
if (psi_tilde > Math.PI)
{
psi_tilde -= 2 * Math.PI;
}
else if (psi_tilde < -Math.PI)
{
psi_tilde += 2 * Math.PI;
}
e_psi = MathTools.AngleDiff(psi, psi_s);
e_psi = -psi_tilde;
//Kx = 1;
//Ky=.1;
//Kz=1.5;
//double psi_a=1/2;
//%Kdelta = .1;
double theta_1 = kz * (ez) / (V * Math.Cos(theta_s));
if (theta_1 > 1)
{
theta_1 = 1;
}
else if (theta_1 < -1)
{
theta_1 = -1;
}
theta = Math.Asin(theta_1);
double sint = Math.Sin(theta);
double cost = Math.Sin(theta);
//double alpha = V*Math.Sin(theta)*Math.Cos(theta_s)+V*Math.Cos(theta)*Math.Cos(theta_s)*Math.Cos(e_psi) + V*ey/ex*Math.Cos(theta)*Math.Sin(e_psi)+V*ez/ex*Math.Cos(theta)*Math.Sin(theta_s)*Math.Sin(e_psi);
double alpha = V * sint * sint_s + V * cost * cost_s * Math.Cos(e_psi);
double delta_ey = -psi_a * (Math.Exp(2 * kdelta * ey) - 1) / (Math.Exp(2 * kdelta * ey) + 1);
double delta_ey_dot = -(4 * psi_a * kdelta * Math.Exp(2 * kdelta * ey)) / Math.Pow(Math.Exp(2 * kdelta * ey) + 1, 2);
s_dot = (kx * ex + alpha);
s_dot = 0;
//double beta = -k_s*s_dot+delta_ey_dot*V*Math.Cos(theta)*Math.Sin(e_psi)-(ey*delta_ey)/(ey-delta_ey);
double pBeta = (ey * V * cost * Math.Sin(e_psi) + V * ez * cost * sint_s * Math.Cos(e_psi) - V * ey * Math.Sin(delta_ey)) / (ey - delta_ey);
double beta = -k_s * s_dot - delta_ey_dot * (k_s * s_dot * ex * cost_s + k_s * s_dot * ez * sint_s + V * cost * Math.Sin(e_psi)) + pBeta;
phi = Math.Atan(V / g * (-beta - ky * (e_psi - delta_ey)));
if (phi > 0.8)
{
phi = 0.8;
}
else if (phi < -0.8)
{
phi = -0.8;
}
if (theta > 0.8)
{
theta = 0.8;
}
else if (theta < -0.8)
{
theta = -0.8;
}
if (s_dot > 20)
{
s_dot = 20;
}
}