public void LQRTest()
{
RectangleMatrix target = new RectangleMatrix(5, 2); // TODO: инициализация подходящего значения
Vector v = new Vector(5); // TODO: инициализация подходящего значения
target[0, 0] = 0;
target[1, 0] = 1;
target[2, 0] = 2;
target[3, 0] = 3;
target[4, 0] = 4;
target[0, 1] = 1;
target[1, 1] = 1;
target[2, 1] = 1;
target[3, 1] = 1;
target[4, 1] = 1;
v[0] = 0;
v[1] = 2;
v[2] = 0;
v[3] = 2;
v[4] = 0;
Vector actual;
actual = target.LQR(v);
Assert.AreEqual(Math.Round(actual[0], 1), 0.0);
Assert.AreEqual(Math.Round(actual[1], 1), 0.8);
}
public void op_SubtractionTest()
{
RectangleMatrix matrix1 = new RectangleMatrix(3, 2); // TODO: инициализация подходящего значения
matrix1[0, 0] = 1.0;
matrix1[0, 1] = 2.0;
matrix1[1, 0] = 3.0;
matrix1[1, 1] = 4.0;
matrix1[2, 0] = 5.0;
matrix1[2, 1] = 6.0;
RectangleMatrix matrix2 = new RectangleMatrix(3, 2); // TODO: инициализация подходящего значения
matrix2[0, 0] = 1.0;
matrix2[0, 1] = 2.0;
matrix2[1, 0] = 3.0;
matrix2[1, 1] = 4.0;
matrix2[2, 0] = 5.0;
matrix2[2, 1] = 6.0;
RectangleMatrix expected = new RectangleMatrix(3, 2); // TODO: инициализация подходящего значения
expected[0, 0] = 0.0;
expected[0, 1] = 0.0;
expected[1, 0] = 0.0;
expected[1, 1] = 0.0;
expected[2, 0] = 0.0;
expected[2, 1] = 0.0;
RectangleMatrix actual;
actual = (matrix1 - matrix2);
Assert.AreEqual(expected, actual);
}
public void op_MultiplyTest()
{
RectangleMatrix matrix1 = new RectangleMatrix(3, 2); // TODO: инициализация подходящего значения
matrix1[0, 0] = 1.0;
matrix1[0, 1] = 2.0;
matrix1[1, 0] = 3.0;
matrix1[1, 1] = 4.0;
matrix1[2, 0] = 5.0;
matrix1[2, 1] = 6.0;
RectangleMatrix matrix2 = new RectangleMatrix(2, 3); // TODO: инициализация подходящего значения
matrix2[0, 0] = 1.0;
matrix2[1, 0] = 2.0;
matrix2[0, 1] = 3.0;
matrix2[1, 1] = 4.0;
matrix2[0, 2] = 5.0;
matrix2[1, 2] = 6.0;
RectangleMatrix expected = new RectangleMatrix(2, 2); // TODO: инициализация подходящего значения
expected[0, 0] = 35.0;
expected[0, 1] = 44.0;
expected[1, 0] = 44.0;
expected[1, 1] = 56.0;
RectangleMatrix actual;
actual = (matrix2 * matrix1);
Assert.AreEqual(expected, actual);
}
public void op_MultiplyTest()
{
RectangleMatrix matrix = new RectangleMatrix(3, 2); // TODO: инициализация подходящего значения
matrix[0, 0] = 1.0;
matrix[1, 0] = 2.0;
matrix[2, 0] = 3.0;
matrix[0, 1] = 4.0;
matrix[1, 1] = 5.0;
matrix[2, 1] = 6.0;
Vector vector = new Vector(2); // TODO: инициализация подходящего значения
vector[0] = 1.0;
vector[1] = 2.0;
Vector expected = new Vector(3); // TODO: инициализация подходящего значения
expected[0] = 9.0;
expected[1] = 12.0;
expected[2] = 15.0;
Vector actual;
actual = (matrix * vector);
Assert.AreEqual(expected, actual);
}
public virtual void LQR(double[] X, double[] Y)
{
if (X.Length != Y.Length)
{
return;
}
int N = X.Length;
int M = order;
RectangleMatrix A = new RectangleMatrix(N, M);
Vector B = new Vector(N);
for (int n = 0; n < N; n++)
{
B[n] = C;
for (int m = 0; m < M; m++)
{
A[n, m] = phi[m](X[n], Y[n]);
}
}
Vector V = A.LQR(B);
for (int n = 0; n < order; n++)
{
a[n] = V[n];
}
}
private void Update()
{
if ((StartPoint == null) | (EndPoint == null))
{
return;
}
if ((StartDirection == null) | (EndDirection == null))
{
return;
}
SquareMatrix M = new SquareMatrix(order + 1);
RectangleMatrix A = new RectangleMatrix(order + 1, dim);
RectangleMatrix B = new RectangleMatrix(order + 1, dim);
double t0 = 0.0;
double t1 = 1.0;
for (int n = 0; n <= order; n++)
{
M[0, n] = phi[n](t0);
M[1, n] = phi[n](t1);
M[2, n] = phis[n](t0);
M[3, n] = phis[n](t1);
}
for (int n = 0; n < dim; n++)
{
B[0, n] = StartPoint.q[n];
B[1, n] = EndPoint.q[n];
B[2, n] = StartDirection[n];
B[3, n] = EndDirection[n];
}
A = M.invert() * B;
for (int m = 0; m < dim; m++)
{
for (int n = 0; n <= order; n++)
{
a[n].q[m] = A[n, m];
}
}
if (Changed != null)
{
Changed(this, EventArgs.Empty);
}
}
public override void LQR(double[] X, double[] Y)
{
if (X.Length != Y.Length)
{
return;
}
int N = X.Length;
int M = order - 1;
RectangleMatrix A = new RectangleMatrix(N, M);
Vector B = new Vector(N);
for (int n = 0; n < N; n++)
{
B[n] = -phi[0](X[n], Y[n]);
for (int m = 0; m < M; m++)
{
A[n, m] = phi[m + 1](X[n], Y[n]);
}
}
Vector V = A.LQR(B);
for (int n = 0; n < M; n++)
{
a[n + 1] = V[n];
}
a[0] = 1.0;
x0 = a[1];
y0 = a[2];
R = Math.Sqrt(x0 * x0 + y0 * y0 - a[3]);
}
