public AddPage()
{
InitializeComponent();
button1.Content = new ImageButton();
button2.Content = new ImageButtonDelete();
_sampleA = new ListBoxMatrixItem(new MatrixView(2, 2, false) { MatrixName = "SampleA" });
_sampleB = new ListBoxMatrixItem(new MatrixView(3, 3, false) { MatrixName = "SampleB" });
_listboxMatrixItems = new List<ListBoxMatrixItem> {_sampleA, _sampleB};
if (IsolatedStorageSettings.ApplicationSettings.Contains("userData"))
{
List<PortableMatrix> matrixList = IsolatedStorageSettings.ApplicationSettings["userData"] as List<PortableMatrix>;
if (matrixList != null)
foreach (PortableMatrix matrix in matrixList)
{
MatrixLibrary.Matrix newMatrix = new MatrixLibrary.Matrix(matrix.ToTwoDimensionalArray(matrix.MatrixArray));
MatrixView mView = new MatrixView(newMatrix.NoCols, newMatrix.NoRows, false);
mView.UpdateMatrix(newMatrix);
mView.MatrixName = matrix.MatrixName;
_listboxMatrixItems.Add(new ListBoxMatrixItem(mView));
}
}
listBox1.ItemsSource = _listboxMatrixItems;
this.listBox1.SelectedItem = _sampleB;
ApplicationBar = new ApplicationBar {IsVisible = true, Mode = ApplicationBarMode.Default, Opacity = 0.5};
ApplicationBarIconButton saveButton = new ApplicationBarIconButton(){IconUri = new Uri("/Image/save.png", UriKind.Relative), IsEnabled = true, Text = "Save"};
saveButton.Click += new EventHandler(SaveButtonClick);
ApplicationBar.Buttons.Add(saveButton);
}
public static double[] getAngleFromRotation(MatrixLibrary.Matrix m)
{
/** this conversion uses conventions as described on page:
* http://www.euclideanspace.com/maths/geometry/rotations/euler/index.htm
* Coordinate System: right hand
* Positive angle: right hand
* Order of euler angles: heading first, then attitude, then bank
* matrix row column ordering:
* [m[0,0] m[0,1] m[0,2]]
* [m[1,0] m[1,1] m[1,2]]
* [m[2,0] m[2,1] m[2,2]]*/
// Assuming the angles are in radians.
double yaw = 0, pitch = 0, roll = 0;
if (m[1, 0] > 0.998) // singularity at north pole
{
yaw = Math.Atan2(m[0, 2], m[2, 2]);
pitch = Math.PI / 2;
roll = 0;
return(new double[] { roll, pitch, yaw });
}
if (m[1, 0] < -0.998) // singularity at south pole
{
yaw = Math.Atan2(m[0, 2], m[2, 2]);
pitch = -Math.PI / 2;
roll = 0;
return(new double[] { roll, pitch, yaw });
}
yaw = Math.Atan2(-m[2, 0], m[0, 0]);
roll = Math.Atan2(-m[1, 2], m[1, 1]);
pitch = Math.Asin(m[1, 0]);
return(new double[] { roll, yaw, pitch });
}
public MatrixLibrary.Matrix getQuaternionAsVector()
{
MatrixLibrary.Matrix result = new MatrixLibrary.Matrix(4, 1);
result[0, 0] = q1;
result[1, 0] = q2;
result[2, 0] = q3;
result[3, 0] = q4;
return(result);
}
/// <summary>
/// This is called when the game should draw itself.
/// </summary>
/// <param name="gameTime">
/// Provides a snapshot of timing values.</param>
protected override void Draw(GameTime gameTime)
{
graphics.GraphicsDevice.Clear(Color.CornflowerBlue);
// Draw the background.
// Start building the sprite.
spriteBatch.Begin(SpriteBlendMode.AlphaBlend);
// Draw the background.
spriteBatch.Draw(background, mainFrame, Color.White);
// End building the sprite.
spriteBatch.End();
cubeEffect.Begin();
if (algorithm == 0) //Complementary filter
{
double[] anglesFiltered = new double[3];
anglesFiltered[0] = filter.getFilteredAngles()[0, 0];
anglesFiltered[1] = filter.getFilteredAngles()[1, 0];
anglesFiltered[2] = filter.getFilteredAngles()[2, 0];
MatrixLibrary.Matrix anglesMatrix = new MatrixLibrary.Matrix(3, 1);
anglesMatrix[0, 0] = anglesFiltered[0];
anglesMatrix[1, 0] = anglesFiltered[1];
anglesMatrix[2, 0] = anglesFiltered[2];
MatrixLibrary.Matrix q = MyQuaternion.getQuaternionFromAngles(anglesMatrix);
worldMatrix = Matrix.CreateFromQuaternion(AuxFrame * new Quaternion((float)q[1, 0], -(float)q[2, 0], (float)q[3, 0], -(float)q[0, 0]));
//worldMatrix = Matrix.CreateFromYawPitchRoll((float)(anglesFiltered[1] * Math.PI / 180), -(float)(anglesFiltered[0] * Math.PI / 180), -(float)(anglesFiltered[2] * Math.PI / 180));
}
else
{
float q1 = (float)filter.getFilteredQuaternions()[1, 0];
float q2 = (float)filter.getFilteredQuaternions()[2, 0];
float q3 = (float)filter.getFilteredQuaternions()[3, 0];
float q0 = (float)filter.getFilteredQuaternions()[0, 0];
if (trend)
{
plotForm.AddDataToGraph(q0, q1, q2, q3);
}
worldMatrix = Matrix.CreateFromQuaternion(AuxFrame * new Quaternion(q1, q2, q3, q0));
}
cubeEffect.World = worldMatrix;
foreach (EffectPass pass in cubeEffect.CurrentTechnique.Passes)
{
pass.Begin();
cubeEffect.Texture = cube.shapeTexture;
cube.RenderShape(GraphicsDevice);
pass.End();
}
cubeEffect.End();
base.Draw(gameTime);
}
public static MatrixLibrary.Matrix getRotationMatrixFromQuaternion2(MatrixLibrary.Matrix Quaternion)
{
MatrixLibrary.Matrix result = new MatrixLibrary.Matrix(1, 9);
result[0, 0] = -1.0f + 2.0f * Quaternion[0, 0] * Quaternion[0, 0] + 2.0f * Quaternion[0, 1] * Quaternion[0, 1];
result[0, 1] = 2.0f * Quaternion[0, 1] * Quaternion[0, 2] + 2.0f * Quaternion[0, 3] * Quaternion[0, 0];
result[0, 2] = 2.0f * Quaternion[0, 1] * Quaternion[0, 3] - 2.0f * Quaternion[0, 2] * Quaternion[0, 0];
result[0, 3] = 2.0f * Quaternion[0, 1] * Quaternion[0, 2] - 2.0f * Quaternion[0, 3] * Quaternion[0, 0];
result[0, 4] = -1.0f + 2.0f * Quaternion[0, 0] * Quaternion[0, 0] + 2.0f * Quaternion[0, 2] * Quaternion[0, 2];
result[0, 5] = 2.0f * Quaternion[0, 2] * Quaternion[0, 3] + 2.0f * Quaternion[0, 1] * Quaternion[0, 0];
result[0, 6] = 2.0f * Quaternion[0, 1] * Quaternion[0, 3] + 2.0f * Quaternion[0, 2] * Quaternion[0, 0];
result[0, 7] = 2.0f * Quaternion[0, 2] * Quaternion[0, 3] - 2.0f * Quaternion[0, 1] * Quaternion[0, 0];
result[0, 8] = -1.0f + 2.0f * Quaternion[0, 0] * Quaternion[0, 0] + 2.0f * Quaternion[0, 3] * Quaternion[0, 3];
return(result);
}
public static MatrixLibrary.Matrix getAnglesFromQuaternion(MatrixLibrary.Matrix q)
{
double q0 = q[0, 0];
double q1 = q[1, 0];
double q2 = q[2, 0];
double q3 = q[3, 0];
MatrixLibrary.Matrix result = new MatrixLibrary.Matrix(3, 1);
result[0, 0] = Math.Atan2((2 * q2 * q3 + 2 * q0 * q1), (1 - 2 * q1 * q1 - 2 * q2 * q2)) * 180.0 / Math.PI;
result[1, 0] = Math.Asin(-2 * q1 * q3 + 2 * q0 * q2) * 180 / Math.PI;
result[2, 0] = Math.Atan2((2 * q1 * q2 + 2 * q0 * q3), (1 - 2 * (q2 * q2 + q3 * q3))) * 180.0 / Math.PI;
return result;
}
public static MatrixLibrary.Matrix getAnglesFromQuaternion(MatrixLibrary.Matrix q)
{
double q0 = q[0, 0];
double q1 = q[1, 0];
double q2 = q[2, 0];
double q3 = q[3, 0];
MatrixLibrary.Matrix result = new MatrixLibrary.Matrix(3, 1);
result[0, 0] = Math.Atan2((2 * q2 * q3 + 2 * q0 * q1), (1 - 2 * q1 * q1 - 2 * q2 * q2)) * 180.0 / Math.PI;
result[1, 0] = Math.Asin(-2 * q1 * q3 + 2 * q0 * q2) * 180 / Math.PI;
result[2, 0] = Math.Atan2((2 * q1 * q2 + 2 * q0 * q3), (1 - 2 * (q2 * q2 + q3 * q3))) * 180.0 / Math.PI;
return(result);
}
public static MatrixLibrary.Matrix getQuaternionFromAngles2(MatrixLibrary.Matrix angle)
{
/*
* Example
* we take the 90 degree rotation from this: to this:
*
* As shown here the axis angle for this rotation is:
*
* heading = 0 degrees
* bank = 90 degrees
* attitude = 0 degrees
*
* so substituting this in the above formula gives:
*
* c1 = cos(heading / 2) = 1
* c2 = cos(attitude / 2) = 1
* c3 = cos(bank / 2) = 0.7071
* s1 = sin(heading / 2) = 0
* s2 = sin(attitude / 2) = 0
* s3 = sin(bank / 2) = 0.7071
*
* w = c1 c2 c3 - s1 s2 s3 = 0.7071
* x = s1 s2 c3 +c1 c2 s3 = 0.7071
* y = s1 c2 c3 + c1 s2 s3 = 0
* z = c1 s2 c3 - s1 c2 s3 = 0
*
* which gives the quaternion 0.7071 + i 0.7071
*/
MatrixLibrary.Matrix result = new MatrixLibrary.Matrix(4, 1);
angle = (angle * Math.PI) / 180.0;
double c1 = Math.Cos(angle[2, 0] / 2);
double c2 = Math.Cos(angle[1, 0] / 2);
double c3 = Math.Cos(angle[0, 0] / 2);
double s1 = Math.Sin(angle[2, 0] / 2);
double s2 = Math.Sin(angle[1, 0] / 2);
double s3 = Math.Sin(angle[0, 0] / 2);
result[0, 0] = c1 * c2 * c3 - s1 * s2 * s3;
result[1, 0] = s1 * s2 * c3 + c1 * c2 * s3;
result[2, 0] = s1 * c2 * c3 + c1 * s2 * s3;
result[3, 0] = c1 * s2 * c3 - s1 * c2 * s3;
return(result);
}
public static MatrixLibrary.Matrix getQuaternionFromAngles(MatrixLibrary.Matrix angle)
{
MatrixLibrary.Matrix result = new MatrixLibrary.Matrix(4, 1);
angle = (angle * Math.PI) / 180.0;
double c1 = Math.Cos(angle[0, 0] / 2);
double c2 = Math.Cos(angle[1, 0] / 2);
double c3 = Math.Cos(angle[2, 0] / 2);
double s1 = Math.Sin(angle[0, 0] / 2);
double s2 = Math.Sin(angle[1, 0] / 2);
double s3 = Math.Sin(angle[2, 0] / 2);
result[0, 0] = c1 * c2 * c3 + s1 * s2 * s3;
result[1, 0] = s1 * c2 * c3 - c1 * s2 * s3;
result[2, 0] = c1 * s2 * c3 + s1 * c2 * s3;
result[3, 0] = c1 * c2 * s3 - s1 * s2 * c3;
return(result);
}
public static MatrixLibrary.Matrix quaternionProduct(MatrixLibrary.Matrix quaternion, MatrixLibrary.Matrix matrix)
{
double a1 = quaternion[0, 0];
double a2 = quaternion[1, 0];
double a3 = quaternion[2, 0];
double a4 = quaternion[3, 0];
double b1 = matrix[0, 0];
double b2 = matrix[1, 0];
double b3 = matrix[2, 0];
double b4 = matrix[3, 0];
MatrixLibrary.Matrix result = new MatrixLibrary.Matrix(4, 1);
result[0, 0] = a1 * b1 - a2 * b2 - a3 * b3 - a4 * b4;
result[1, 0] = a1 * b2 + a2 * b1 + a3 * b4 - a4 * b3;
result[2, 0] = a1 * b3 - a2 * b4 + a3 * b1 + a4 * b2;
result[3, 0] = a1 * b4 + a2 * b3 - a3 * b2 + a4 * b1;
return(result);
}
protected MatrixLibrary.Matrix magneticCompensation(MyQuaternion q, double m_x, double m_y, double m_z)
{
MatrixLibrary.Matrix h = new MatrixLibrary.Matrix(4, 1);
MatrixLibrary.Matrix temp;
//compute the direction of the magnetic field
MatrixLibrary.Matrix quaternion = q.getQuaternionAsVector();
MatrixLibrary.Matrix quaternion_conjugate = q.getConjugate();
//magnetic field compensation
temp = MyQuaternion.quaternionProduct(quaternion, new MyQuaternion(0.0, m_x, m_y, m_z).getQuaternionAsVector());
h = MyQuaternion.quaternionProduct(temp, quaternion_conjugate);
double bx = Math.Sqrt((h[1, 0] * h[1, 0] + h[2, 0] * h[2, 0]));
double bz = h[3, 0];
double norm = Math.Sqrt(bx * bx + bz * bz);
bx /= norm;
bz /= norm;
MatrixLibrary.Matrix result = new MatrixLibrary.Matrix(3, 1);
result[0, 0] = bx;
result[1, 0] = 0;
result[2, 0] = bz;
return(result);
}
protected MatrixLibrary.Matrix gaussNewtonMethod(double a_x, double a_y, double a_z, double m_x, double m_y, double m_z, MatrixLibrary.Matrix qObserv)
{
double norm;
MatrixLibrary.Matrix n = new MatrixLibrary.Matrix(4, 1);
MatrixLibrary.Matrix bRif = new MatrixLibrary.Matrix(3, 1);
MatrixLibrary.Matrix jacobian = new MatrixLibrary.Matrix(6, 4);
MatrixLibrary.Matrix R = new MatrixLibrary.Matrix(6, 6);
MatrixLibrary.Matrix y_e = new MatrixLibrary.Matrix(6, 1);
MatrixLibrary.Matrix y_b = new MatrixLibrary.Matrix(6, 1);
double a = qObserv[1, 0];
double b = qObserv[2, 0];
double c = qObserv[3, 0];
double d = qObserv[0, 0];
int i = 0;
MatrixLibrary.Matrix n_k = new MatrixLibrary.Matrix(4, 1);
n_k[0, 0] = a;
n_k[1, 0] = b;
n_k[2, 0] = c;
n_k[3, 0] = d;
//Magnetometer Calibration values
m_x = m_x * magnSFX + magnOffX;
m_y = m_y * magnSFY + magnOffY;
m_z = m_z * magnSFZ + magnOffZ;
norm = Math.Sqrt(m_x * m_x + m_y * m_y + m_z * m_z);
m_x /= norm;
m_y /= norm;
m_z /= norm;
while (i < 3)
{
MyQuaternion q = new MyQuaternion(d, a, b, c);
bRif = magneticCompensation(q, m_x, m_y, m_z);
double bx = bRif[0, 0];
double by = bRif[1, 0];
double bz = bRif[2, 0];
//Jacobian Computation
double j11 = (2 * a * a_x + 2 * b * a_y + 2 * c * a_z);
double j12 = (-2 * b * a_x + 2 * a * a_y + 2 * d * a_z);
double j13 = (-2 * c * a_x - 2 * d * a_y + 2 * a * a_z);
double j14 = (2 * d * a_x - 2 * c * a_y + 2 * b * a_z);
double j21 = (2 * b * a_x - 2 * a * a_y - 2 * d * a_z);
double j22 = (2 * a * a_x + 2 * b * a_y + 2 * c * a_z);
double j23 = (2 * d * a_x - 2 * c * a_y + 2 * b * a_z);
double j24 = (2 * c * a_x + 2 * d * a_y - 2 * a * a_z);
double j31 = (2 * c * a_x + 2 * d * a_y - 2 * a * a_z);
double j32 = (-2 * d * a_x + 2 * c * a_y - 2 * b * a_z);
double j33 = (2 * a * a_x + 2 * b * a_y + 2 * c * a_z);
double j34 = (-2 * b * a_x + 2 * a * a_y + 2 * d * a_z);
double j41 = (2 * a * m_x + 2 * b * m_y + 2 * c * m_z);
double j42 = (-2 * b * m_x + 2 * a * m_y + 2 * m_z * d);
double j43 = (-2 * c * m_x - 2 * d * m_y + 2 * a * m_z);
double j44 = (2 * d * m_x - 2 * c * m_y + 2 * b * m_z);
double j51 = (2 * b * m_x - 2 * a * m_y - 2 * d * m_z);
double j52 = (2 * a * m_x + 2 * b * m_y + 2 * c * m_z);
double j53 = (2 * d * m_x - 2 * c * m_y + 2 * b * m_z);
double j54 = (2 * c * m_x + 2 * d * m_y - 2 * a * m_z);
double j61 = (2 * c * m_x + 2 * d * m_y - 2 * a * m_z);
double j62 = (-2 * d * m_x + 2 * c * m_y - 2 * b * m_z);
double j63 = (2 * a * m_x + 2 * b * m_y + 2 * c * m_z);
double j64 = (-2 * b * m_x + 2 * a * m_y + 2 * d * m_z);
jacobian[0, 0] = j11;
jacobian[0, 1] = j12;
jacobian[0, 2] = j13;
jacobian[0, 3] = j14;
jacobian[1, 0] = j21;
jacobian[1, 1] = j22;
jacobian[1, 2] = j23;
jacobian[1, 3] = j24;
jacobian[2, 0] = j31;
jacobian[2, 1] = j32;
jacobian[2, 2] = j33;
jacobian[2, 3] = j34;
jacobian[3, 0] = j41;
jacobian[3, 1] = j42;
jacobian[3, 2] = j43;
jacobian[3, 3] = j44;
jacobian[4, 0] = j51;
jacobian[4, 1] = j52;
jacobian[4, 2] = j53;
jacobian[4, 3] = j54;
jacobian[5, 0] = j61;
jacobian[5, 1] = j62;
jacobian[5, 2] = j63;
jacobian[5, 3] = j64;
jacobian = -1 * jacobian;
//End Jacobian Computation
//DCM Rotation Matrix
R[0, 0] = d * d + a * a - b * b - c * c;
R[0, 1] = 2 * (a * b - c * d);
R[0, 2] = 2 * (a * c + b * d);
R[1, 0] = 2 * (a * b + c * d);
R[1, 1] = d * d + b * b - a * a - c * c;
R[1, 2] = 2 * (b * c - a * d);
R[2, 0] = 2 * (a * c - b * d);
R[2, 1] = 2 * (b * c + a * d);
R[2, 2] = d * d + c * c - b * b - a * a;
R[3, 3] = d * d + a * a - b * b - c * c;
R[3, 4] = 2 * (a * b - c * d);
R[3, 5] = 2 * (a * c + b * d);
R[4, 3] = 2 * (a * b + c * d);
R[4, 4] = d * d + b * b - a * a - c * c;
R[4, 5] = 2 * (b * c - a * d);
R[5, 3] = 2 * (a * c - b * d);
R[5, 4] = 2 * (b * c + a * d);
R[5, 5] = d * d + c * c - b * b - a * a;
R[3, 0] = 0;
R[3, 1] = 0;
R[3, 2] = 0;
R[4, 0] = 0;
R[4, 1] = 0;
R[4, 2] = 0;
R[5, 0] = 0;
R[5, 1] = 0;
R[5, 2] = 0;
R[0, 3] = 0;
R[0, 4] = 0;
R[0, 5] = 0;
R[1, 3] = 0;
R[1, 4] = 0;
R[1, 5] = 0;
R[2, 3] = 0;
R[2, 4] = 0;
R[2, 5] = 0;
//End DCM
//Reference Vector
y_e[0, 0] = 0;
y_e[1, 0] = 0;
y_e[2, 0] = 1;
y_e[3, 0] = bx;
y_e[4, 0] = by;
y_e[5, 0] = bz;
//Body frame Vector
y_b[0, 0] = a_x;
y_b[1, 0] = a_y;
y_b[2, 0] = a_z;
y_b[3, 0] = m_x;
y_b[4, 0] = m_y;
y_b[5, 0] = m_z;
//Gauss Newton Step
n = n_k - MatrixLibrary.Matrix.Inverse((MatrixLibrary.Matrix.Transpose(jacobian) * jacobian)) * MatrixLibrary.Matrix.Transpose(jacobian) * (y_e - R * y_b);
double normGauss = Math.Sqrt(n[0, 0] * n[0, 0] + n[1, 0] * n[1, 0] + n[2, 0] * n[2, 0] + n[3, 0] * n[3, 0]);
n /= normGauss;
//Console.Out.WriteLine(n + " "+ norm);
a = n[0, 0];
b = n[1, 0];
c = n[2, 0];
d = n[3, 0];
n_k[0, 0] = a;
n_k[1, 0] = b;
n_k[2, 0] = c;
n_k[3, 0] = d;
i++;
}
return(new MyQuaternion(d, a, b, c).getQuaternionAsVector());
}
public static MatrixLibrary.Matrix getQuaternionFromAngles2(MatrixLibrary.Matrix angle)
{
/*
Example
we take the 90 degree rotation from this: to this:
As shown here the axis angle for this rotation is:
heading = 0 degrees
bank = 90 degrees
attitude = 0 degrees
so substituting this in the above formula gives:
c1 = cos(heading / 2) = 1
c2 = cos(attitude / 2) = 1
c3 = cos(bank / 2) = 0.7071
s1 = sin(heading / 2) = 0
s2 = sin(attitude / 2) = 0
s3 = sin(bank / 2) = 0.7071
w = c1 c2 c3 - s1 s2 s3 = 0.7071
x = s1 s2 c3 +c1 c2 s3 = 0.7071
y = s1 c2 c3 + c1 s2 s3 = 0
z = c1 s2 c3 - s1 c2 s3 = 0
which gives the quaternion 0.7071 + i 0.7071
*/
MatrixLibrary.Matrix result = new MatrixLibrary.Matrix(4, 1);
angle = (angle * Math.PI) / 180.0;
double c1 = Math.Cos(angle[2, 0] / 2);
double c2 = Math.Cos(angle[1, 0] / 2);
double c3 = Math.Cos(angle[0, 0] / 2);
double s1 = Math.Sin(angle[2, 0] / 2);
double s2 = Math.Sin(angle[1, 0] / 2);
double s3 = Math.Sin(angle[0, 0] / 2);
result[0, 0] = c1 * c2 * c3 - s1 * s2 * s3;
result[1, 0] = s1 * s2 * c3 + c1 * c2 * s3;
result[2, 0] = s1 * c2 * c3 + c1 * s2 * s3;
result[3, 0] = c1 * s2 * c3 - s1 * c2 * s3;
return result;
}
private void LoadMatrix_Click(object sender, EventArgs e)
{
//матрицца для вывода
MatrixLibrary.Matrix DownloadedForFall = null;
//флажок
bool notFailed = true;
switch (this.MatrixBox.Text)
{
case "An":
{
DownloadedForFall = sys.current_state.analog_A(Convert.ToInt32(TimeBox.Text), Convert.ToInt32(DelayBox.Text));
break;
}
case "Bn":
{
DownloadedForFall = sys.current_state.analog_B(Convert.ToInt32(TimeBox.Text), Convert.ToInt32(DelayBox.Text));
break;
}
case "A":
{
DownloadedForFall = sys.current_state.get_A(Convert.ToInt32(TimeBox.Text), Convert.ToInt32(DelayBox.Text));
break;
}
case "B":
{
DownloadedForFall = sys.current_state.B(Convert.ToInt32(TimeBox.Text), Convert.ToInt32(DelayBox.Text));
break;
}
case "H":
{
DownloadedForFall = sys.current_state.H(Convert.ToInt32(TimeBox.Text));
break;
}
case "L":
{
DownloadedForFall = new MatrixLibrary.Matrix(1, sys.current_state.a);
for (int i = 0; i < sys.current_state.a; i++)
{
DownloadedForFall[0, i] = sys.current_state.L[i];
}
break;
}
case "M":
{
DownloadedForFall = new MatrixLibrary.Matrix(1, sys.current_state.c);
for (int i = 0; i < sys.current_state.c; i++)
{
DownloadedForFall[0, i] = sys.current_state.M[i];
}
break;
}
case "tau":
{
DownloadedForFall = new MatrixLibrary.Matrix(1, sys.current_state.tau.Count);
for (int i = 0; i < sys.current_state.tau.Count; i++)
{
DownloadedForFall[0, i] = sys.current_state.tau[i];
}
break;
}
case "teta":
{
DownloadedForFall = new MatrixLibrary.Matrix(1, sys.current_state.teta.Count);
for (int i = 0; i < sys.current_state.teta.Count; i++)
{
DownloadedForFall[0, i] = sys.current_state.teta[i];
}
break;
}
case "Fi":
{
DownloadedForFall = sys.current_state.Fi();
break;
}
case "Psi":
{
DownloadedForFall = sys.current_state.Psi();
break;
}
default:
{
notFailed = false;
break;
}
}
//инициализация Матрицорисователя
if (notFailed)//если конечно ктото не перепорол
{
this.UniversalMatrixBox.ReloadMatrix(DownloadedForFall);
UniversalMatrixBox.Visible = true;
//if(this.Size.Width < UniversalMatrixBox.Location.X + UniversalMatrixBox.Size.Width || this.Size.Height < UniversalMatrixBox.Location.Y + UniversalMatrixBox.Size.Height)
// this.Size = new Size(UniversalMatrixBox.Location) + UniversalMatrixBox.Size + new Size(10,30);
//if (UniversalMatrixBox.Location.X + UniversalMatrixBox.Size.Width < 450 && UniversalMatrixBox.Location.Y + UniversalMatrixBox.Size.Height <= 200)
// this.Size = new Size(450, 200);
}
}
public static MatrixLibrary.Matrix getRotationMatrixFromQuaternion2(MatrixLibrary.Matrix Quaternion)
{
MatrixLibrary.Matrix result = new MatrixLibrary.Matrix(1, 9);
result[0, 0] = -1.0f + 2.0f * Quaternion[0, 0] * Quaternion[0, 0] +2.0f * Quaternion[0, 1] * Quaternion[0, 1];
result[0, 1] = 2.0f * Quaternion[0, 1] * Quaternion[0, 2] + 2.0f * Quaternion[0, 3] * Quaternion[0, 0];
result[0, 2] = 2.0f * Quaternion[0, 1] * Quaternion[0, 3] - 2.0f * Quaternion[0, 2] * Quaternion[0, 0];
result[0, 3] = 2.0f * Quaternion[0, 1] * Quaternion[0, 2] - 2.0f * Quaternion[0, 3] * Quaternion[0, 0];
result[0, 4] = -1.0f +2.0f * Quaternion[0, 0] * Quaternion[0, 0] + 2.0f * Quaternion[0, 2] * Quaternion[0, 2];
result[0, 5] = 2.0f * Quaternion[0, 2] * Quaternion[0, 3] + 2.0f * Quaternion[0, 1] * Quaternion[0, 0];
result[0, 6] = 2.0f * Quaternion[0, 1] * Quaternion[0, 3] + 2.0f * Quaternion[0, 2] * Quaternion[0, 0];
result[0, 7] = 2.0f * Quaternion[0, 2] * Quaternion[0, 3] - 2.0f * Quaternion[0, 1] * Quaternion[0, 0];
result[0, 8] = -1.0f +2.0f * Quaternion[0, 0] * Quaternion[0, 0] + 2.0f * Quaternion[0, 3] * Quaternion[0,3 ];
return result;
}
public static MatrixLibrary.Matrix quaternionProduct(MyQuaternion q_a, MyQuaternion q_b)
{
MatrixLibrary.Matrix v1 = q_a.getQuaternionAsVector();
MatrixLibrary.Matrix v2 = q_b.getQuaternionAsVector();
return(quaternionProduct(v1, v2));
}
public MatrixLibrary.Matrix getQuaternionAsVector()
{
MatrixLibrary.Matrix result = new MatrixLibrary.Matrix(4, 1);
result[0, 0] = q1;
result[1, 0] = q2;
result[2, 0] = q3;
result[3, 0] = q4;
return result;
}
public static MatrixLibrary.Matrix quaternionProduct(MatrixLibrary.Matrix quaternion, MatrixLibrary.Matrix matrix)
{
double a1 = quaternion[0, 0];
double a2 = quaternion[1, 0];
double a3 = quaternion[2, 0];
double a4 = quaternion[3, 0];
double b1 = matrix[0, 0];
double b2 = matrix[1, 0];
double b3 = matrix[2, 0];
double b4 = matrix[3, 0];
MatrixLibrary.Matrix result = new MatrixLibrary.Matrix(4, 1);
result[0, 0] = a1 * b1 - a2 * b2 - a3 * b3 - a4 * b4;
result[1, 0] = a1 * b2 + a2 * b1 + a3 * b4 - a4 * b3;
result[2, 0] = a1 * b3 - a2 * b4 + a3 * b1 + a4 * b2;
result[3, 0] = a1 * b4 + a2 * b3 - a3 * b2 + a4 * b1;
return result;
}
public QuaternionCF()
{
k = 0.98;
aAcc = new List<double>();
bAcc = new List<double>();
aMagn = new List<double>();
bMagn = new List<double>();
aAcc.Add(1);
aAcc.Add(-2.9529);
aAcc.Add(2.9069);
aAcc.Add(-0.954);
bAcc.Add(0.000001597);
bAcc.Add(0.000004792);
bAcc.Add(0.000004792);
bAcc.Add(0.000001597);
accFilter = new IIRFilter(aAcc, bAcc);
aMagn.Add(1);
aMagn.Add(-1.73);
aMagn.Add(0.76);
bMagn.Add(0.0078);
bMagn.Add(0.0156);
bMagn.Add(0.0078);
magnFilter = new IIRFilter(aMagn, bMagn);
AccObservX = new List<double>();
AccObservY = new List<double>();
AccObservZ = new List<double>();
AccFiltX = new List<double>();
AccFiltY = new List<double>();
AccFiltZ = new List<double>();
MagnObservX = new List<double>();
MagnObservY = new List<double>();
MagnObservZ = new List<double>();
MagnFiltX = new List<double>();
MagnFiltY = new List<double>();
MagnFiltZ = new List<double>();
qFilt = new MatrixLibrary.Matrix(4, 1);
qGyroFilt = new MatrixLibrary.Matrix(4, 1);
qObserv = new MatrixLibrary.Matrix(4, 1);
qFilt[0, 0] = 1;
qFilt[1, 0] = 0;
qFilt[2, 0] = 0;
qFilt[3, 0] = 0;
qGyroFilt[0, 0] = 1;
qGyroFilt[1, 0] = 0;
qGyroFilt[2, 0] = 0;
qGyroFilt[3, 0] = 0;
qObserv[0, 0] = 1;
qObserv[1, 0] = 0;
qObserv[2, 0] = 0;
qObserv[3, 0] = 0;
}
public static MatrixLibrary.Matrix getQuaternionFromAngles(MatrixLibrary.Matrix angle)
{
MatrixLibrary.Matrix result = new MatrixLibrary.Matrix(4, 1);
angle = (angle * Math.PI) / 180.0;
double c1 = Math.Cos(angle[0, 0] / 2);
double c2 = Math.Cos(angle[1, 0] / 2);
double c3 = Math.Cos(angle[2, 0] / 2);
double s1 = Math.Sin(angle[0, 0] / 2);
double s2 = Math.Sin(angle[1, 0] / 2);
double s3 = Math.Sin(angle[2, 0] / 2);
result[0, 0] = c1 * c2 * c3 + s1 * s2 * s3;
result[1, 0] = ( s1 * c2 * c3 - c1 * s2 * s3);
result[2, 0] = c1 * s2 * c3 + s1 * c2 * s3;
result[3, 0] = c1 * c2 * s3 - s1 * s2 * c3;
return result;
}
public override void filterStep(double w_x, double w_y, double w_z, double a_x, double a_y, double a_z, double m_x, double m_y, double m_z)
{
// normalise the accelerometer measurement
double norm = Math.Sqrt(a_x * a_x + a_y * a_y + a_z * a_z);
a_x /= norm;
a_y /= norm;
a_z /= norm;
// normalise the magnetometer measurement
norm = Math.Sqrt(m_x * m_x + m_y * m_y + m_z * m_z);
m_x /= norm;
m_y /= norm;
m_z /= norm;
if (AccObservX.Count < 10)
{
AccObservX.Add(a_x);
AccObservY.Add(a_y);
AccObservZ.Add(a_z);
}
else
{
AccObservX.RemoveAt(0);
AccObservY.RemoveAt(0);
AccObservZ.RemoveAt(0);
AccObservX.Add(a_x);
AccObservY.Add(a_y);
AccObservZ.Add(a_z);
}
if (MagnObservX.Count < 10)
{
MagnObservX.Add(m_x);
MagnObservY.Add(m_y);
MagnObservZ.Add(m_z);
}
else
{
MagnObservX.RemoveAt(0);
MagnObservY.RemoveAt(0);
MagnObservZ.RemoveAt(0);
MagnObservX.Add(m_x);
MagnObservY.Add(m_y);
MagnObservZ.Add(m_z);
//Filter stabilization
accFilter.Applyfilter(AccObservX, out AccFiltX);
accFilter.Applyfilter(AccObservY, out AccFiltY);
accFilter.Applyfilter(AccObservZ, out AccFiltZ);
magnFilter.Applyfilter(MagnObservX, out MagnFiltX);
magnFilter.Applyfilter(MagnObservY, out MagnFiltY);
magnFilter.Applyfilter(MagnObservZ, out MagnFiltZ);
}
if (step < 10)
{
}
else
{
accFilter.Applyfilter(AccObservX, out AccFiltX);
accFilter.Applyfilter(AccObservY, out AccFiltY);
accFilter.Applyfilter(AccObservZ, out AccFiltZ);
magnFilter.Applyfilter(MagnObservX, out MagnFiltX);
magnFilter.Applyfilter(MagnObservY, out MagnFiltY);
magnFilter.Applyfilter(MagnObservZ, out MagnFiltZ);
a_x = AccFiltX.Last();
a_y = AccFiltY.Last();
a_z = AccFiltZ.Last();
m_x = MagnFiltX.Last();
m_y = MagnFiltY.Last();
m_z = MagnFiltZ.Last();
//Magnetometer Calibration values
//m_x =m_x*1.2+ 0.3;
//m_y = m_y*1.1+0.04;
//m_z += -0.08;
double normA = Math.Sqrt(a_x * a_x + a_y * a_y + a_z * a_z);
double normM = Math.Sqrt(m_x * m_x + m_y * m_y + m_z * m_z);
a_x /= normA;
a_y /= normA;
a_z /= normA;
m_x /= normM;
m_y /= normM;
m_z /= normM;
w_x = (w_x - gyroOffRoll) * Math.PI / 180;
w_y = (w_y - gyroOffPitch) * Math.PI / 180;
w_z = (w_z - gyroOffYaw) * Math.PI / 180;
MatrixLibrary.Matrix dq = 0.5 * (MyQuaternion.quaternionProduct(qFilt, new MyQuaternion(0, w_x, w_y, w_z).getQuaternionAsVector()));
qGyroFilt = qFilt + dq * dt;
norm = Math.Sqrt(qGyroFilt[0, 0] * qGyroFilt[0, 0] + qGyroFilt[1, 0] * qGyroFilt[1, 0] + qGyroFilt[2, 0] * qGyroFilt[2, 0] + qGyroFilt[3, 0] * qGyroFilt[3, 0]);
qGyroFilt /= norm;
double dqNorm = Math.Sqrt(dq[0, 0] * dq[0, 0] + dq[1, 0] * dq[1, 0] + dq[2, 0] * dq[2, 0] + dq[3, 0] * dq[3, 0]);
double mu = 10 * dqNorm * dt;
if (this.obsMethod == 0)
{
qObserv = GradientDescent(a_x, a_y, a_z, m_x, m_y, m_z, mu, qFilt);
}
else
{
qObserv = gaussNewtonMethod(a_x, a_y, a_z, m_x, m_y, m_z, qFilt);
}
qFilt = qGyroFilt * k + qObserv * (1 - k);
norm = Math.Sqrt(qFilt[0, 0] * qFilt[0, 0] + qFilt[1, 0] * qFilt[1, 0] + qFilt[2, 0] * qFilt[2, 0] + qFilt[3, 0] * qFilt[3, 0]);
qFilt /= norm;
}
step++;
}
protected MatrixLibrary.Matrix magneticCompensation(MyQuaternion q, double m_x, double m_y, double m_z)
{
MatrixLibrary.Matrix h = new MatrixLibrary.Matrix(4, 1);
MatrixLibrary.Matrix temp;
//compute the direction of the magnetic field
MatrixLibrary.Matrix quaternion = q.getQuaternionAsVector();
MatrixLibrary.Matrix quaternion_conjugate = q.getConjugate();
//magnetic field compensation
temp = MyQuaternion.quaternionProduct(quaternion, new MyQuaternion(0.0, m_x, m_y, m_z).getQuaternionAsVector());
h = MyQuaternion.quaternionProduct(temp, quaternion_conjugate);
double bx = Math.Sqrt((h[1, 0] * h[1, 0] + h[2, 0] * h[2, 0]));
double bz = h[3, 0];
double norm = Math.Sqrt(bx * bx + bz * bz);
bx /= norm;
bz /= norm;
MatrixLibrary.Matrix result = new MatrixLibrary.Matrix(3, 1);
result[0, 0] = bx;
result[1, 0] = 0;
result[2, 0] = bz;
return result;
}
//A quadratic interger program for graph matching,
//When tuned with the right similarity function it can represent the GED problem.
// See Pattern Reocgnition Paper paper On the unification of graph matching and graph edit distance paper.
//The directed version has not been tested yet
public override void QAPGMGED()
{
//some important variables
int nbNode1 = graph1.ListNodes.Count;
int nbNode2 = graph2.ListNodes.Count;
int nbvar = nbNode1 * nbNode2; // number of variables
int nbcontraintes = nbNode1 + nbNode2; // number of constraints
Graphs.Label nodeepslabel;
//Adjacency matrices
MatrixLibrary.Matrix adj1 = graph1.calculateAdjacencyMatrix();
MatrixLibrary.Matrix adj2 = graph2.calculateAdjacencyMatrix();
//Similarity matrix
Double[,] S = new Double[nbvar, nbvar];
//Creation of the Similarity matrix
//4 for loops integrated
int countrow = -1;
for (int i = 0; i < nbNode1; i++)
{
Node nodei = graph1.ListNodes[i];
for (int k = 0; k < nbNode2; k++)
{
Node nodek = graph2.ListNodes[k];
countrow = countrow + 1;
int countcol = -1;
for (int j = 0; j < nbNode1; j++)
{
Node nodej = graph1.ListNodes[j];
for (int l = 0; l < nbNode2; l++)
{
Node nodel = graph2.ListNodes[l];
countcol = countcol + 1;
if (i == j && k == l) // Similarity between nodes
{
Type typeLabel = nodei.Label.GetType();
object obj = Activator.CreateInstance(typeLabel);
nodeepslabel = (Graphs.Label)obj;
nodeepslabel.Id = ConstantsAC.EPS_ID;
double costsub = nodei.Label.dissimilarity(nodek.Label);
double costdel = nodei.Label.dissimilarity(nodeepslabel);
double costins = nodeepslabel.dissimilarity(nodek.Label);
double cost = costsub - costdel - costins;
cost *= -1;
S[countrow, countcol] = cost;
}
else // Similarity between edges
{
int connect1 = (int)adj1[i, j];
int connect2 = (int)adj2[k, l];
if (connect1 == 1 && connect2 == 1) // Two edges are connected
{
Edge ij = findedge(nodei, nodej);
Edge kl = findedge(nodek, nodel);
Type typeLabel = ij.Label.GetType();
object obj = Activator.CreateInstance(typeLabel);
nodeepslabel = (Graphs.Label)obj;
nodeepslabel.Id = ConstantsAC.EPS_ID;
double costsub = ij.Label.dissimilarity(kl.Label);
double costdel = ij.Label.dissimilarity(nodeepslabel);
double costins = nodeepslabel.dissimilarity(kl.Label);
double cost = costsub - costdel - costins;
cost *= -1;
//cost *= 0.5;
S[countrow, countcol] = cost;
}
}
}
}
}
}
//We compute the constant that represents the
//deletion and insertion of the nodes and edges
//deletions of the nodes of g1
double constante = 0;
for (int i = 1; i <= nbNode1; i++)
{
Type typeLabel = graph1.ListNodes[i - 1].Label.GetType();
object obj = Activator.CreateInstance(typeLabel);
nodeepslabel = (Graphs.Label)obj;
nodeepslabel.Id = ConstantsAC.EPS_ID;
constante += (graph1.ListNodes[i - 1].Label).dissimilarity(nodeepslabel);
}
//deletions of the edges of g1
for (int ij = 1; ij <= nbEdge1; ij++)
{
Type typeLabel = graph1.ListEdges[ij - 1].Label.GetType();
object obj = Activator.CreateInstance(typeLabel);
nodeepslabel = (Graphs.Label)obj;
nodeepslabel.Id = ConstantsAC.EPS_ID;
double costEdge = (graph1.ListEdges[ij - 1].Label).dissimilarity(nodeepslabel);
constante += costEdge;
}
//insertions of the nodes of g2
for (int k = 1; k <= nbNode2; k++)
{
Type typeLabel = graph2.ListNodes[k - 1].Label.GetType();
object obj = Activator.CreateInstance(typeLabel);
nodeepslabel = (Graphs.Label)obj;
nodeepslabel.Id = ConstantsAC.EPS_ID;
constante += (graph2.ListNodes[k - 1].Label).dissimilarity(nodeepslabel);
}
//insertions of the edges of g2
for (int kl = 1; kl <= nbEdge2; kl++)
{
Type typeLabel = graph2.ListEdges[kl - 1].Label.GetType();
object obj = Activator.CreateInstance(typeLabel);
nodeepslabel = (Graphs.Label)obj;
nodeepslabel.Id = ConstantsAC.EPS_ID;
double costEdge = (graph2.ListEdges[kl - 1].Label).dissimilarity(nodeepslabel);
constante += costEdge;
}
// the number of variables is the number of columns
int nbCols = nbvar;
// the number of constraints is the number of rows
int nbRows = nbcontraintes;
List <string> colNameList = new List <string>();
//We create the name of the vrariables for all couples of nodes in g1 and g2
for (int i = 0; i < nbNode1; i++)
{
for (int k = 0; k < nbNode2; k++)
{
colNameList.Add("x_" + graph1.ListNodes[i].Id + "," + graph2.ListNodes[k].Id + "");
}
}
try
{
cplex = new Cplex(); // creation du modèle CPLEX
this.ilpMatrix = cplex.AddLPMatrix(); // matrice du pb (initialisée à 0 colonnes et 0 lignes)
ColumnArray colonnes = cplex.ColumnArray(ilpMatrix, nbCols); // définition des variables-colonnes du pb
// ajout des variables-colonnes dans la matrice du pb
// y is a vector
INumVar[] y = cplex.NumVarArray(colonnes, 0, 1, NumVarType.Bool, colNameList.ToArray());
#region création fonction objectif
// CALCUL DES COEFFICIENTS
// We create the ojective function
INumExpr[] coeffs_expr = new INumExpr[nbvar * nbvar]; // vecteur des coeffs des coûts des arrêtes
int count = 0;
for (int ik = 0; ik < nbvar; ik++)
{
for (int jl = 0; jl < nbvar; jl++)
{
coeffs_expr[count] = cplex.Prod(y[ik], y[jl]);
coeffs_expr[count] = cplex.Prod(S[ik, jl], coeffs_expr[count]);
count = count + 1;
}
}
INumExpr ff = cplex.Sum(coeffs_expr);
INumExpr gg = cplex.Sum(ff, cplex.Constant(-constante));
cplex.AddMaximize(gg);
#endregion
#region définition des contraintes du pb
// We create the constraints
// The sum of x variables by rows
for (int i = 0; i < nbNode1; i++)
{
INumVar[] sommeLignes = new INumVar[nbNode2];
for (int k = 0; k < nbNode2; k++)
{
string nameVarik = "x_" + i + "," + k;
int indexik = SolverCPLEX.GetIndexByName(y, nameVarik);
sommeLignes[k] = y[indexik];
}
cplex.AddLe(cplex.Sum(sommeLignes), 1);
}
// The sum of x variables by columns
for (int k = 0; k < nbNode2; k++)
{
INumVar[] sommeLignes = new INumVar[nbNode1];
for (int i = 0; i < nbNode1; i++)
{
string nameVarik = "x_" + i + "," + k;
int indexik = SolverCPLEX.GetIndexByName(y, nameVarik);
sommeLignes[i] = y[indexik];
}
cplex.AddLe(cplex.Sum(sommeLignes), 1);
}
#endregion
}
catch (ILOG.Concert.Exception e)
{
System.Console.WriteLine("Concert exception `" + e + "` caught");
}
}
protected MatrixLibrary.Matrix GradientDescent(double a_x, double a_y, double a_z, double m_x, double m_y, double m_z, double mu,MatrixLibrary.Matrix qObserv)
{
int i = 0;
double q1, q2, q3, q4;
MatrixLibrary.Matrix f_obb = new MatrixLibrary.Matrix(6, 1);
MatrixLibrary.Matrix Jacobian = new MatrixLibrary.Matrix(6, 4);
MatrixLibrary.Matrix Df = new MatrixLibrary.Matrix(4, 1);
double norm;
double bx, bz, by;
MatrixLibrary.Matrix result = new MatrixLibrary.Matrix(4, 1);
q1 = qObserv[0, 0];
q2 = qObserv[1, 0];
q3 = qObserv[2, 0];
q4 = qObserv[3, 0];
//Magnetometer Calibration values
m_x = m_x *magnSFX+ magnOffX;
m_y = m_y *magnSFY+ magnOffY;
m_z= m_z *magnSFZ+ magnOffZ;
norm = Math.Sqrt(m_x * m_x + m_y * m_y + m_z * m_z);
m_x /= norm;
m_y /= norm;
m_z /= norm;
while (i < 10)
{
//compute the direction of the magnetic field
MatrixLibrary.Matrix quaternion = new MyQuaternion(q1, q2, q3, q4).getQuaternionAsVector();
MatrixLibrary.Matrix bRif = magneticCompensation(new MyQuaternion(q1, q2, q3, q4), m_x, m_y, m_z);
bx = bRif[0, 0];
by = bRif[1, 0];
bz = bRif[2, 0];
//compute the objective functions
f_obb[0, 0] = 2 * (q2 * q4 - q1 * q3) - a_x;
f_obb[1, 0] = 2 * (q1 * q2 + q3 * q4) - a_y;
f_obb[2, 0] = 2 * (0.5 - q2 * q2 - q3 * q3) - a_z;
f_obb[3, 0] = 2 * bx * (0.5 - q3 * q3 - q4 * q4) + 2 * by * (q1 * q4 + q2 * q3) + 2 * bz * (q2 * q4 - q1 * q3) - m_x;
f_obb[4, 0] = 2 * bx * (q2 * q3 - q1 * q4) + 2 * by * (0.5 - q2 * q2 - q4 * q4) + 2 * bz * (q1 * q2 + q3 * q4) - m_y;
f_obb[5, 0] = 2 * bx * (q1 * q3 + q2 * q4) + 2 * by * (q3 * q4 - q1 * q2) + 2 * bz * (0.5 - q2 * q2 - q3 * q3) - m_z;
//compute the jacobian
Jacobian[0, 0] = -2 * q3;
Jacobian[0, 1] = 2 * q4;
Jacobian[0, 2] = -2 * q1;
Jacobian[0, 3] = 2 * q2;
Jacobian[1, 0] = 2 * q2;
Jacobian[1, 1] = 2 * q1;
Jacobian[1, 2] = 2 * q4;
Jacobian[1, 3] = 2 * q3;
Jacobian[2, 0] = 0;
Jacobian[2, 1] = -4 * q2;
Jacobian[2, 2] = -4 * q3;
Jacobian[2, 3] = 0;
Jacobian[3, 0] = 2 * by * q4 - 2 * bz * q3;
Jacobian[3, 1] = 2 * by * q3 + 2 * bz * q4;
Jacobian[3, 2] = -4 * bx * q3 + 2 * by * q2 - 2 * bz * q1;
Jacobian[3, 3] = -4 * bx * q4 + 2 * by * q1 + 2 * bz * q2;
Jacobian[4, 0] = -2 * bx * q4 + 2 * bz * q2;
Jacobian[4, 1] = 2 * bx * q3 - 4 * by * q2 + 2 * bz * q1;
Jacobian[4, 2] = 2 * bx * q2 + 2 * bz * q4;
Jacobian[4, 3] = -2 * bx * q1 - 4 * by * q4 + 2 * bz * q3;
Jacobian[5, 0] = 2 * bx * q3 - 2 * by * q2;
Jacobian[5, 1] = 2 * bx * q4 - 2 * by * q1 - 4 * bz * q2;
Jacobian[5, 2] = 2 * bx * q1 + 2 * by * q4 - 4 * bz * q3;
Jacobian[5, 3] = 2 * bx * q2 + 2 * by * q3;
Df = MatrixLibrary.Matrix.Multiply(MatrixLibrary.Matrix.Transpose(Jacobian), f_obb);
norm = Math.Sqrt(Df[0, 0] * Df[0, 0] + Df[1, 0] * Df[1, 0] + Df[2, 0] * Df[2, 0] + Df[3, 0] * Df[3, 0]);
Df = MatrixLibrary.Matrix.ScalarDivide(norm, Df);
result = quaternion - mu * Df;
q1 = result[0, 0];
q2 = result[1, 0];
q3 = result[2, 0];
q4 = result[3, 0];
norm = Math.Sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4);
result = MatrixLibrary.Matrix.ScalarDivide(norm, result);
q1 = result[0, 0];
q2 = result[1, 0];
q3 = result[2, 0];
q4 = result[3, 0];
i = i + 1;
}
return result;
}
protected MatrixLibrary.Matrix GradientDescent(double a_x, double a_y, double a_z, double m_x, double m_y, double m_z, double mu, MatrixLibrary.Matrix qObserv)
{
int i = 0;
double q1, q2, q3, q4;
MatrixLibrary.Matrix f_obb = new MatrixLibrary.Matrix(6, 1);
MatrixLibrary.Matrix Jacobian = new MatrixLibrary.Matrix(6, 4);
MatrixLibrary.Matrix Df = new MatrixLibrary.Matrix(4, 1);
double norm;
double bx, bz, by;
MatrixLibrary.Matrix result = new MatrixLibrary.Matrix(4, 1);
q1 = qObserv[0, 0];
q2 = qObserv[1, 0];
q3 = qObserv[2, 0];
q4 = qObserv[3, 0];
//Magnetometer Calibration values
m_x = m_x * magnSFX + magnOffX;
m_y = m_y * magnSFY + magnOffY;
m_z = m_z * magnSFZ + magnOffZ;
norm = Math.Sqrt(m_x * m_x + m_y * m_y + m_z * m_z);
m_x /= norm;
m_y /= norm;
m_z /= norm;
while (i < 10)
{
//compute the direction of the magnetic field
MatrixLibrary.Matrix quaternion = new MyQuaternion(q1, q2, q3, q4).getQuaternionAsVector();
MatrixLibrary.Matrix bRif = magneticCompensation(new MyQuaternion(q1, q2, q3, q4), m_x, m_y, m_z);
bx = bRif[0, 0];
by = bRif[1, 0];
bz = bRif[2, 0];
//compute the objective functions
f_obb[0, 0] = 2 * (q2 * q4 - q1 * q3) - a_x;
f_obb[1, 0] = 2 * (q1 * q2 + q3 * q4) - a_y;
f_obb[2, 0] = 2 * (0.5 - q2 * q2 - q3 * q3) - a_z;
f_obb[3, 0] = 2 * bx * (0.5 - q3 * q3 - q4 * q4) + 2 * by * (q1 * q4 + q2 * q3) + 2 * bz * (q2 * q4 - q1 * q3) - m_x;
f_obb[4, 0] = 2 * bx * (q2 * q3 - q1 * q4) + 2 * by * (0.5 - q2 * q2 - q4 * q4) + 2 * bz * (q1 * q2 + q3 * q4) - m_y;
f_obb[5, 0] = 2 * bx * (q1 * q3 + q2 * q4) + 2 * by * (q3 * q4 - q1 * q2) + 2 * bz * (0.5 - q2 * q2 - q3 * q3) - m_z;
//compute the jacobian
Jacobian[0, 0] = -2 * q3;
Jacobian[0, 1] = 2 * q4;
Jacobian[0, 2] = -2 * q1;
Jacobian[0, 3] = 2 * q2;
Jacobian[1, 0] = 2 * q2;
Jacobian[1, 1] = 2 * q1;
Jacobian[1, 2] = 2 * q4;
Jacobian[1, 3] = 2 * q3;
Jacobian[2, 0] = 0;
Jacobian[2, 1] = -4 * q2;
Jacobian[2, 2] = -4 * q3;
Jacobian[2, 3] = 0;
Jacobian[3, 0] = 2 * by * q4 - 2 * bz * q3;
Jacobian[3, 1] = 2 * by * q3 + 2 * bz * q4;
Jacobian[3, 2] = -4 * bx * q3 + 2 * by * q2 - 2 * bz * q1;
Jacobian[3, 3] = -4 * bx * q4 + 2 * by * q1 + 2 * bz * q2;
Jacobian[4, 0] = -2 * bx * q4 + 2 * bz * q2;
Jacobian[4, 1] = 2 * bx * q3 - 4 * by * q2 + 2 * bz * q1;
Jacobian[4, 2] = 2 * bx * q2 + 2 * bz * q4;
Jacobian[4, 3] = -2 * bx * q1 - 4 * by * q4 + 2 * bz * q3;
Jacobian[5, 0] = 2 * bx * q3 - 2 * by * q2;
Jacobian[5, 1] = 2 * bx * q4 - 2 * by * q1 - 4 * bz * q2;
Jacobian[5, 2] = 2 * bx * q1 + 2 * by * q4 - 4 * bz * q3;
Jacobian[5, 3] = 2 * bx * q2 + 2 * by * q3;
Df = MatrixLibrary.Matrix.Multiply(MatrixLibrary.Matrix.Transpose(Jacobian), f_obb);
norm = Math.Sqrt(Df[0, 0] * Df[0, 0] + Df[1, 0] * Df[1, 0] + Df[2, 0] * Df[2, 0] + Df[3, 0] * Df[3, 0]);
Df = MatrixLibrary.Matrix.ScalarDivide(norm, Df);
result = quaternion - mu * Df;
q1 = result[0, 0];
q2 = result[1, 0];
q3 = result[2, 0];
q4 = result[3, 0];
norm = Math.Sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4);
result = MatrixLibrary.Matrix.ScalarDivide(norm, result);
q1 = result[0, 0];
q2 = result[1, 0];
q3 = result[2, 0];
q4 = result[3, 0];
i = i + 1;
}
return(result);
}
protected MatrixLibrary.Matrix gaussNewtonMethod(double a_x, double a_y, double a_z, double m_x, double m_y, double m_z,MatrixLibrary.Matrix qObserv)
{
double norm;
MatrixLibrary.Matrix n = new MatrixLibrary.Matrix(4, 1);
MatrixLibrary.Matrix bRif = new MatrixLibrary.Matrix(3, 1);
MatrixLibrary.Matrix jacobian = new MatrixLibrary.Matrix(6, 4);
MatrixLibrary.Matrix R = new MatrixLibrary.Matrix(6, 6);
MatrixLibrary.Matrix y_e = new MatrixLibrary.Matrix(6, 1);
MatrixLibrary.Matrix y_b = new MatrixLibrary.Matrix(6, 1);
double a = qObserv[1, 0];
double b = qObserv[2, 0];
double c = qObserv[3, 0];
double d = qObserv[0, 0];
int i = 0;
MatrixLibrary.Matrix n_k = new MatrixLibrary.Matrix(4, 1);
n_k[0, 0] = a;
n_k[1, 0] = b;
n_k[2, 0] = c;
n_k[3, 0] = d;
//Magnetometer Calibration values
m_x = m_x * magnSFX + magnOffX;
m_y = m_y * magnSFY + magnOffY;
m_z = m_z * magnSFZ + magnOffZ;
norm = Math.Sqrt(m_x * m_x + m_y * m_y + m_z * m_z);
m_x /= norm;
m_y /= norm;
m_z /= norm;
while (i < 3)
{
MyQuaternion q = new MyQuaternion(d, a, b, c);
bRif = magneticCompensation(q, m_x, m_y, m_z);
double bx = bRif[0, 0];
double by = bRif[1, 0];
double bz = bRif[2, 0];
//Jacobian Computation
double j11 = (2 * a * a_x + 2 * b * a_y + 2 * c * a_z);
double j12 = (-2 * b * a_x + 2 * a * a_y + 2 * d * a_z);
double j13 = (-2 * c * a_x - 2 * d * a_y + 2 * a * a_z);
double j14 = (2 * d * a_x - 2 * c * a_y + 2 * b * a_z);
double j21 = (2 * b * a_x - 2 * a * a_y - 2 * d * a_z);
double j22 = (2 * a * a_x + 2 * b * a_y + 2 * c * a_z);
double j23 = (2 * d * a_x - 2 * c * a_y + 2 * b * a_z);
double j24 = (2 * c * a_x + 2 * d * a_y - 2 * a * a_z);
double j31 = (2 * c * a_x + 2 * d * a_y - 2 * a * a_z);
double j32 = (-2 * d * a_x + 2 * c * a_y - 2 * b * a_z);
double j33 = (2 * a * a_x + 2 * b * a_y + 2 * c * a_z);
double j34 = (-2 * b * a_x + 2 * a * a_y + 2 * d * a_z);
double j41 = (2 * a * m_x + 2 * b * m_y + 2 * c * m_z);
double j42 = (-2 * b * m_x + 2 * a * m_y + 2 * m_z * d);
double j43 = (-2 * c * m_x - 2 * d * m_y + 2 * a * m_z);
double j44 = (2 * d * m_x - 2 * c * m_y + 2 * b * m_z);
double j51 = (2 * b * m_x - 2 * a * m_y - 2 * d * m_z);
double j52 = (2 * a * m_x + 2 * b * m_y + 2 * c * m_z);
double j53 = (2 * d * m_x - 2 * c * m_y + 2 * b * m_z);
double j54 = (2 * c * m_x + 2 * d * m_y - 2 * a * m_z);
double j61 = (2 * c * m_x + 2 * d * m_y - 2 * a * m_z);
double j62 = (-2 * d * m_x + 2 * c * m_y - 2 * b * m_z);
double j63 = (2 * a * m_x + 2 * b * m_y + 2 * c * m_z);
double j64 = (-2 * b * m_x + 2 * a * m_y + 2 * d * m_z);
jacobian[0, 0] = j11;
jacobian[0, 1] = j12;
jacobian[0, 2] = j13;
jacobian[0, 3] = j14;
jacobian[1, 0] = j21;
jacobian[1, 1] = j22;
jacobian[1, 2] = j23;
jacobian[1, 3] = j24;
jacobian[2, 0] = j31;
jacobian[2, 1] = j32;
jacobian[2, 2] = j33;
jacobian[2, 3] = j34;
jacobian[3, 0] = j41;
jacobian[3, 1] = j42;
jacobian[3, 2] = j43;
jacobian[3, 3] = j44;
jacobian[4, 0] = j51;
jacobian[4, 1] = j52;
jacobian[4, 2] = j53;
jacobian[4, 3] = j54;
jacobian[5, 0] = j61;
jacobian[5, 1] = j62;
jacobian[5, 2] = j63;
jacobian[5, 3] = j64;
jacobian = -1 * jacobian;
//End Jacobian Computation
//DCM Rotation Matrix
R[0, 0] = d * d + a * a - b * b - c * c;
R[0, 1] = 2 * (a * b - c * d);
R[0, 2] = 2 * (a * c + b * d);
R[1, 0] = 2 * (a * b + c * d);
R[1, 1] = d * d + b * b - a * a - c * c;
R[1, 2] = 2 * (b * c - a * d);
R[2, 0] = 2 * (a * c - b * d);
R[2, 1] = 2 * (b * c + a * d);
R[2, 2] = d * d + c * c - b * b - a * a;
R[3, 3] = d * d + a * a - b * b - c * c;
R[3, 4] = 2 * (a * b - c * d);
R[3, 5] = 2 * (a * c + b * d);
R[4, 3] = 2 * (a * b + c * d);
R[4, 4] = d * d + b * b - a * a - c * c;
R[4, 5] = 2 * (b * c - a * d);
R[5, 3] = 2 * (a * c - b * d);
R[5, 4] = 2 * (b * c + a * d);
R[5, 5] = d * d + c * c - b * b - a * a;
R[3, 0] = 0;
R[3, 1] = 0;
R[3, 2] = 0;
R[4, 0] = 0;
R[4, 1] = 0;
R[4, 2] = 0;
R[5, 0] = 0;
R[5, 1] = 0;
R[5, 2] = 0;
R[0, 3] = 0;
R[0, 4] = 0;
R[0, 5] = 0;
R[1, 3] = 0;
R[1, 4] = 0;
R[1, 5] = 0;
R[2, 3] = 0;
R[2, 4] = 0;
R[2, 5] = 0;
//End DCM
//Reference Vector
y_e[0, 0] = 0;
y_e[1, 0] = 0;
y_e[2, 0] = 1;
y_e[3, 0] = bx;
y_e[4, 0] = by;
y_e[5, 0] = bz;
//Body frame Vector
y_b[0, 0] = a_x;
y_b[1, 0] = a_y;
y_b[2, 0] = a_z;
y_b[3, 0] = m_x;
y_b[4, 0] = m_y;
y_b[5, 0] = m_z;
//Gauss Newton Step
n = n_k - MatrixLibrary.Matrix.Inverse((MatrixLibrary.Matrix.Transpose(jacobian) * jacobian)) * MatrixLibrary.Matrix.Transpose(jacobian) * (y_e - R * y_b);
double normGauss = Math.Sqrt(n[0, 0] * n[0, 0] + n[1, 0] * n[1, 0] + n[2, 0] * n[2, 0] + n[3, 0] * n[3, 0]);
n /= normGauss;
//Console.Out.WriteLine(n + " "+ norm);
a = n[0, 0];
b = n[1, 0];
c = n[2, 0];
d = n[3, 0];
n_k[0, 0] = a;
n_k[1, 0] = b;
n_k[2, 0] = c;
n_k[3, 0] = d;
i++;
}
return new MyQuaternion(d, a, b, c).getQuaternionAsVector();
}
public override void filterStep(double w_x, double w_y, double w_z, double a_x, double a_y, double a_z, double m_x, double m_y, double m_z)
{
// normalise the accelerometer measurement
double norm = Math.Sqrt(a_x * a_x + a_y * a_y + a_z * a_z);
a_x /= norm;
a_y /= norm;
a_z /= norm;
// normalise the magnetometer measurement
norm = Math.Sqrt(m_x * m_x + m_y * m_y + m_z * m_z);
m_x /= norm;
m_y /= norm;
m_z /= norm;
if (AccObservX.Count < 10)
{
AccObservX.Add(a_x);
AccObservY.Add(a_y);
AccObservZ.Add(a_z);
}
else
{
AccObservX.RemoveAt(0);
AccObservY.RemoveAt(0);
AccObservZ.RemoveAt(0);
AccObservX.Add(a_x);
AccObservY.Add(a_y);
AccObservZ.Add(a_z);
}
if (MagnObservX.Count < 10)
{
MagnObservX.Add(m_x);
MagnObservY.Add(m_y);
MagnObservZ.Add(m_z);
}
else
{
MagnObservX.RemoveAt(0);
MagnObservY.RemoveAt(0);
MagnObservZ.RemoveAt(0);
MagnObservX.Add(m_x);
MagnObservY.Add(m_y);
MagnObservZ.Add(m_z);
//Filter stabilization
accFilter.Applyfilter(AccObservX, out AccFiltX);
accFilter.Applyfilter(AccObservY, out AccFiltY);
accFilter.Applyfilter(AccObservZ, out AccFiltZ);
magnFilter.Applyfilter(MagnObservX, out MagnFiltX);
magnFilter.Applyfilter(MagnObservY, out MagnFiltY);
magnFilter.Applyfilter(MagnObservZ, out MagnFiltZ);
}
if (step < 10)
{
}
else
{
accFilter.Applyfilter(AccObservX, out AccFiltX);
accFilter.Applyfilter(AccObservY, out AccFiltY);
accFilter.Applyfilter(AccObservZ, out AccFiltZ);
magnFilter.Applyfilter(MagnObservX, out MagnFiltX);
magnFilter.Applyfilter(MagnObservY, out MagnFiltY);
magnFilter.Applyfilter(MagnObservZ, out MagnFiltZ);
a_x = AccFiltX.Last();
a_y = AccFiltY.Last();
a_z = AccFiltZ.Last();
m_x = MagnFiltX.Last();
m_y = MagnFiltY.Last();
m_z = MagnFiltZ.Last();
//Magnetometer Calibration values
//m_x =m_x*1.2+ 0.3;
//m_y = m_y*1.1+0.04;
//m_z += -0.08;
double normA = Math.Sqrt(a_x * a_x + a_y * a_y + a_z * a_z);
double normM = Math.Sqrt(m_x * m_x + m_y * m_y + m_z * m_z);
a_x /= normA;
a_y /= normA;
a_z /= normA;
m_x /= normM;
m_y /= normM;
m_z /= normM;
w_x = (w_x - gyroOffRoll)*Math.PI/180;
w_y = (w_y - gyroOffPitch) * Math.PI / 180;
w_z = (w_z - gyroOffYaw)*Math.PI/180;
MatrixLibrary.Matrix dq = 0.5 * (MyQuaternion.quaternionProduct(qFilt, new MyQuaternion(0, w_x, w_y, w_z).getQuaternionAsVector()));
qGyroFilt=qFilt + dq * dt;
norm = Math.Sqrt(qGyroFilt[0, 0] * qGyroFilt[0, 0] + qGyroFilt[1, 0] * qGyroFilt[1, 0] + qGyroFilt[2, 0] * qGyroFilt[2, 0] + qGyroFilt[3, 0] * qGyroFilt[3, 0]);
qGyroFilt /= norm;
double dqNorm = Math.Sqrt(dq[0, 0] * dq[0, 0] + dq[1, 0] * dq[1, 0] + dq[2, 0] * dq[2, 0] + dq[3, 0] * dq[3, 0]);
double mu = 10 * dqNorm * dt;
if(this.obsMethod==0)
qObserv = GradientDescent(a_x, a_y, a_z, m_x, m_y, m_z, mu,qFilt);
else
qObserv = gaussNewtonMethod(a_x, a_y, a_z, m_x, m_y, m_z,qFilt);
qFilt = qGyroFilt * k + qObserv * (1 - k);
norm = Math.Sqrt(qFilt[0, 0] * qFilt[0, 0] + qFilt[1, 0] * qFilt[1, 0] + qFilt[2, 0] * qFilt[2, 0] + qFilt[3, 0] * qFilt[3, 0]);
qFilt /= norm;
}
step++;
}
public QuaternionCF()
{
k = 0.98;
aAcc = new List <double>();
bAcc = new List <double>();
aMagn = new List <double>();
bMagn = new List <double>();
aAcc.Add(1);
aAcc.Add(-2.9529);
aAcc.Add(2.9069);
aAcc.Add(-0.954);
bAcc.Add(0.000001597);
bAcc.Add(0.000004792);
bAcc.Add(0.000004792);
bAcc.Add(0.000001597);
accFilter = new IIRFilter(aAcc, bAcc);
aMagn.Add(1);
aMagn.Add(-1.73);
aMagn.Add(0.76);
bMagn.Add(0.0078);
bMagn.Add(0.0156);
bMagn.Add(0.0078);
magnFilter = new IIRFilter(aMagn, bMagn);
AccObservX = new List <double>();
AccObservY = new List <double>();
AccObservZ = new List <double>();
AccFiltX = new List <double>();
AccFiltY = new List <double>();
AccFiltZ = new List <double>();
MagnObservX = new List <double>();
MagnObservY = new List <double>();
MagnObservZ = new List <double>();
MagnFiltX = new List <double>();
MagnFiltY = new List <double>();
MagnFiltZ = new List <double>();
qFilt = new MatrixLibrary.Matrix(4, 1);
qGyroFilt = new MatrixLibrary.Matrix(4, 1);
qObserv = new MatrixLibrary.Matrix(4, 1);
qFilt[0, 0] = 1;
qFilt[1, 0] = 0;
qFilt[2, 0] = 0;
qFilt[3, 0] = 0;
qGyroFilt[0, 0] = 1;
qGyroFilt[1, 0] = 0;
qGyroFilt[2, 0] = 0;
qGyroFilt[3, 0] = 0;
qObserv[0, 0] = 1;
qObserv[1, 0] = 0;
qObserv[2, 0] = 0;
qObserv[3, 0] = 0;
}
// GED formulation from the paper
// https://ieeexplore.ieee.org/document/1642656
public override void BLPjusticehero()
{
int nbNode1 = graph1.ListNodes.Count;
int nbNode2 = graph2.ListNodes.Count;
int N = nbNode1 + nbNode2; // nombre de noeuds du graphe d'édition
// #region
//création matrices de placement standard A0 et A1
Graph gridg1 = new Graph();
Graph gridg2 = new Graph();
Type typeNodeLabel = graph1.ListNodes[0].Label.GetType();
object objNode = Activator.CreateInstance(typeNodeLabel);
Graphs.Label nodeepslabel = (Graphs.Label)objNode;
nodeepslabel.Id = ConstantsAC.EPS_ID;
Type typeEdgeLabel = null;
object objEdge = null;
Graphs.Label edgeepslabel = null;
if (graph1.ListEdges.Count > 0)
{
typeEdgeLabel = graph1.ListEdges[0].Label.GetType();
objEdge = Activator.CreateInstance(typeEdgeLabel);
edgeepslabel = (Graphs.Label)objEdge;
edgeepslabel.Id = ConstantsAC.EPS_ID;
}
if (graph2.ListEdges.Count > 0)
{
typeEdgeLabel = graph2.ListEdges[0].Label.GetType();
objEdge = Activator.CreateInstance(typeEdgeLabel);
edgeepslabel = (Graphs.Label)objEdge;
edgeepslabel.Id = ConstantsAC.EPS_ID;
}
if (graph1.ListEdges.Count == 0 && graph2.ListEdges.Count == 0)
{
Console.Out.WriteLine("error no edges random edge label alkane");
edgeepslabel = (Graphs.Label) new LabelEdgeAlkane();
edgeepslabel.Id = ConstantsAC.EPS_ID;
}
//else
//{
//edgeepslabel = (Graphs.Label)new LabelEdgeAlkane();
//edgeepslabel.Id = ConstantsAC.EPS_ID;
//}
for (int i = 0; i < nbNode1; i++)
{
gridg1.addNode(graph1.ListNodes[i]);
}
for (int i = 0; i < graph1.ListEdges.Count; i++)
{
gridg1.addEdge(graph1.ListEdges[i]);
}
// ajout des noeuds "virtuels" au graphe g1, pour que g1 corresponde au placement standard dans le graphe d'édition (A0)
for (int i = nbNode1; i < N; i++)
{
Node n = new Node((i + 2).ToString());
gridg1.addNode(n);
n.Label = nodeepslabel;
n.Label.Id = ConstantsAC.EPS_ID;
// LabelEdgeLetter
//n.Label = new Graphs.GenericLabel();
//n.Label.Id = n.Id;
}
MatrixLibrary.Matrix A0 = gridg1.calculateAdjacencyMatrix(); // création matrice d'adajcence de g1
// idem pour g2 (A1)
for (int i = 0; i < nbNode2; i++)
{
gridg2.addNode(graph2.ListNodes[i]);
}
for (int i = 0; i < graph2.ListEdges.Count; i++)
{
gridg2.addEdge(graph2.ListEdges[i]);
}
for (int i = nbNode2; i < N; i++)
{
Node n = new Node((i + 2).ToString());
gridg2.addNode(n);
n.Label = nodeepslabel;//new LabelNodeMutagen();
n.Label.Id = ConstantsAC.EPS_ID;
//n.Label = new Graphs.GenericLabel();
//n.Label.Id = n.Id;
}
MatrixLibrary.Matrix A1 = gridg2.calculateAdjacencyMatrix(); // création matrice d'adajcence de g2
// les graphes vont être étiquetés avec un label LabelNodeMolecule (pour les noeuds) et LabelEdgeMolecule (pour les arrêtes)
// cela va nous permettre d'utiliser notre propre méthode "dissimilarity" pour les noeuds et arrêtes
// gridg1.DynamicCastLabel(graph1.ListNodes[0].Label, graph1.ListEdges[0].Label);
// gridg2.DynamicCastLabel(graph2.ListNodes[0].Label, graph2.ListEdges[0].Label);
// nb variables du pb : matrices P(N*N), S(N*N) et T(N*N)
int nbCols = 3 * (N * N);
// nb contraintes du pb
int nbRows = N * N + N + N;
List <double> objList = new List <double>();
List <string> colNameList = new List <string>();
// coût des opérations d'édition des sommets (coeff de P dans la formule de la distance d'édition)
for (int i = 0; i < N; i++)
{
for (int j = 0; j < N; j++)
{
double costNode = gridg1.ListNodes[i].Label.dissimilarity(gridg2.ListNodes[j].Label);
objList.Add(costNode);
//colNameList.Add("P[" + gridg1.ListNodes[i].Id + "][" + gridg2.ListNodes[j].Id + "]");
colNameList.Add("x_" + gridg1.ListNodes[i].Id + "," + gridg2.ListNodes[j].Id + "");
}
}
// coût des opérations d'édition des arrêtes (coeffs de S et T)
//Edge ee = new Edge((0).ToString());
//ee.Label = new LabelEdgeMutagen();
//ee.Label.Id = ConstantsAC.EPS_ID;
// coeff de S
for (int i = 0; i < N; i++)
{
for (int j = 0; j < N; j++)
{
// dans notre cas, l'opération est soit 0->1 ou 1->0 (insertion ou suppression d'une arrête)
// double costNode = ee.Label.dissimilarity(ee.Label) / 2.0;
double costNode = edgeepslabel.dissimilarity(edgeepslabel) / 2.0;
//double costNode = 0.5 / 2.0;
objList.Add(costNode);
colNameList.Add("S[" + gridg1.ListNodes[i].Id + "][" + gridg2.ListNodes[j].Id + "]");
}
}
// coeff de T
for (int i = 0; i < N; i++)
{
for (int j = 0; j < N; j++)
{
// dans notre cas, l'opération est soit 0->1 ou 1->0 (insertion ou suppression d'une arrête)
//double costNode = ee.Label.dissimilarity(ee.Label) / 2.0;
double costNode = edgeepslabel.dissimilarity(edgeepslabel) / 2.0;
// double costNode = 0.5 / 2.0;
objList.Add(costNode);
colNameList.Add("T[" + gridg1.ListNodes[i].Id + "][" + gridg2.ListNodes[j].Id + "]");
}
}
try
{
// Cplex cplex = new Cplex(); // creation du modèle CPLEX
//ILPMatrix lp_matrix = cplex.AddLPMatrix(); // matrice du pb (initialisée à 0 colonnes et 0 lignes)
cplex = new Cplex();
ilpMatrix = cplex.AddLPMatrix();
ColumnArray colonnes = cplex.ColumnArray(ilpMatrix, nbCols); // définition des variables-colonnes du pb
// ajout des variables-colonnes dans la matrice du pb (avec définition des bornes: P[i][j] = 0 ou 1, idem avec S et T)
INumVar[] x = cplex.NumVarArray(colonnes, 0, 1, NumVarType.Bool, colNameList.ToArray());
INumVar[,] P = new INumVar[N, N];
INumVar[,] S = new INumVar[N, N];
INumVar[,] T = new INumVar[N, N];
for (int i = 0; i < N; i++)
{
for (int j = 0; j < N; j++)
{
P[i, j] = x[(N * i) + j];
}
}
// indice de S et T dans x (utilisés lors de la définition des contraintes)
int indicedebutS = N * N; // indice du 1er élément de S dans x
int indicedebutT = 2 * (N * N); // indice du 1er élément de T dans x
for (int i = 0; i < N; i++)
{
for (int j = 0; j < N; j++)
{
S[i, j] = x[indicedebutS + (N * i) + j];
}
}
for (int i = 0; i < N; i++)
{
for (int j = 0; j < N; j++)
{
T[i, j] = x[indicedebutT + (N * i) + j];
}
}
objCoef = objList.ToArray();
// ajout de la fonction objectif du pb
cplex.AddMinimize(cplex.ScalProd(x, objCoef));
// contrainte n°1
for (int i = 0; i < N; i++)
{
int[] coeffsA0 = new int[N]; // vecteur des coeffs ligne i de A0
INumVar[] coeffsP_A1 = new INumVar[N]; // vecteur des coeffs ligne i de P
for (int c = 0; c < N; c++)
{
coeffsA0[c] = (int)A0[i, c];
coeffsP_A1[c] = P[i, c];
}
for (int j = 0; j < N; j++)
{
INumVar[] coeffsP = new INumVar[N]; // vecteur des coeffs colonne j de P
int[] coeffsA1 = new int[N]; // vecteur des coeffs colonne j de A1
for (int c = 0; c < N; c++)
{
coeffsP[c] = P[c, j];
coeffsA1[c] = (int)A1[c, j];
}
cplex.AddEq(cplex.Sum(
cplex.Diff(
cplex.ScalProd(coeffsP, coeffsA0), cplex.ScalProd(coeffsP_A1, coeffsA1)),
cplex.Diff(
S[i, j], T[i, j]))
, 0);
/* (ANCIEN PRODUIT TERME A TERME)
* cplex.AddEq(cplex.Sum(
* cplex.Diff(
* cplex.Prod(A0[i,j],P[i,j]),cplex.Prod(P[i,j],A1[i,j])),
* cplex.Diff(
* S[i, j], T[i, j]))
* , 0); */
}
}
// contrainte n°2 (somme des lignes dans P = 1 et somme des colonnes dans P = 1)
for (int i = 0; i < N; i++)
{
INumVar[] sommeLignes = new INumVar[N];
INumVar[] sommeColonnes = new INumVar[N];
for (int j = 0; j < N; j++)
{
sommeLignes[j] = x[N * i + j];
sommeColonnes[j] = x[N * j + i];
}
cplex.AddEq(cplex.Sum(sommeLignes), 1);
cplex.AddEq(cplex.Sum(sommeColonnes), 1);
}
}
catch (ILOG.Concert.Exception e)
{
System.Console.WriteLine("Concert exception `" + e + "` caught");
}
}
