/// <summary>
/// Run example.
/// </summary>
/// <seealso cref="http://en.wikipedia.org/wiki/Euclidean_vector#Scalar_multiplication">Multiply vector by scalar</seealso>
/// <seealso cref="http://en.wikipedia.org/wiki/Euclidean_vector#Dot_product">Multiply vector by vector (compute the dot product between two vectors)</seealso>
/// <seealso cref="http://en.wikipedia.org/wiki/Euclidean_vector#Addition_and_subtraction">Vector addition and subtraction</seealso>
/// <seealso cref="http://en.wikipedia.org/wiki/Outer_product">Outer Product of two vectors</seealso>
public void Run()
{
// Initialize IFormatProvider to print matrix/vector data
var formatProvider = (CultureInfo)CultureInfo.InvariantCulture.Clone();
formatProvider.TextInfo.ListSeparator = " ";
// Create vector "X"
var vectorX = new DenseVector(new[] { 1.0, 2.0, 3.0, 4.0, 5.0 });
Console.WriteLine(@"Vector X");
Console.WriteLine(vectorX.ToString("#0.00\t", formatProvider));
Console.WriteLine();
// Create vector "Y"
var vectorY = new DenseVector(new[] { 5.0, 4.0, 3.0, 2.0, 1.0 });
Console.WriteLine(@"Vector Y");
Console.WriteLine(vectorY.ToString("#0.00\t", formatProvider));
Console.WriteLine();
// Multiply vector by scalar
// 1. Using Multiply method and getting result into different vector instance
var resultV = vectorX.Multiply(3.0);
Console.WriteLine(@"Multiply vector by scalar using method Multiply. (result = X.Multiply(3.0))");
Console.WriteLine(resultV.ToString("#0.00\t", formatProvider));
Console.WriteLine();
// 2. Using operator "*"
resultV = 3.0 * vectorX;
Console.WriteLine(@"Multiply vector by scalar using operator *. (result = 3.0 * X)");
Console.WriteLine(resultV.ToString("#0.00\t", formatProvider));
Console.WriteLine();
// 3. Using Multiply method and updating vector itself
vectorX.Multiply(3.0, vectorX);
Console.WriteLine(@"Multiply vector by scalar using method Multiply. (X.Multiply(3.0, X))");
Console.WriteLine(vectorX.ToString("#0.00\t", formatProvider));
Console.WriteLine();
// Multiply vector by vector (compute the dot product between two vectors)
// 1. Using operator "*"
var dotProduct = vectorX * vectorY;
Console.WriteLine(@"Dot product between two vectors using operator *. (result = X * Y)");
Console.WriteLine(dotProduct);
Console.WriteLine();
// 2. Using DotProduct method and getting result into different vector instance
dotProduct = vectorX.DotProduct(vectorY);
Console.WriteLine(@"Dot product between two vectors using method DotProduct. (result = X.DotProduct(Y))");
Console.WriteLine(dotProduct.ToString("#0.00\t", formatProvider));
Console.WriteLine();
// Pointwise multiplies vector with another vector
// 1. Using PointwiseMultiply method and getting result into different vector instance
resultV = vectorX.PointwiseMultiply(vectorY);
Console.WriteLine(@"Pointwise multiplies vector with another vector using method PointwiseMultiply. (result = X.PointwiseMultiply(Y))");
Console.WriteLine(resultV.ToString("#0.00\t", formatProvider));
Console.WriteLine();
// 2. Using PointwiseMultiply method and updating vector itself
vectorX.PointwiseMultiply(vectorY, vectorX);
Console.WriteLine(@"Pointwise multiplies vector with another vector using method PointwiseMultiply. (X.PointwiseMultiply(Y, X))");
Console.WriteLine(vectorX.ToString("#0.00\t", formatProvider));
Console.WriteLine();
// Pointwise divide vector with another vector
// 1. Using PointwiseDivide method and getting result into different vector instance
resultV = vectorX.PointwiseDivide(vectorY);
Console.WriteLine(@"Pointwise divide vector with another vector using method PointwiseDivide. (result = X.PointwiseDivide(Y))");
Console.WriteLine(resultV.ToString("#0.00\t", formatProvider));
Console.WriteLine();
// 2. Using PointwiseDivide method and updating vector itself
vectorX.PointwiseDivide(vectorY, vectorX);
Console.WriteLine(@"Pointwise divide vector with another vector using method PointwiseDivide. (X.PointwiseDivide(Y, X))");
Console.WriteLine(vectorX.ToString("#0.00\t", formatProvider));
Console.WriteLine();
// Addition
// 1. Using operator "+"
resultV = vectorX + vectorY;
Console.WriteLine(@"Add vectors using operator +. (result = X + Y)");
Console.WriteLine(resultV.ToString("#0.00\t", formatProvider));
Console.WriteLine();
// 2. Using Add method and getting result into different vector instance
resultV = vectorX.Add(vectorY);
Console.WriteLine(@"Add vectors using method Add. (result = X.Add(Y))");
Console.WriteLine(resultV.ToString("#0.00\t", formatProvider));
Console.WriteLine();
// 3. Using Add method and updating vector itself
vectorX.Add(vectorY, vectorX);
Console.WriteLine(@"Add vectors using method Add. (X.Add(Y, X))");
Console.WriteLine(vectorX.ToString("#0.00\t", formatProvider));
Console.WriteLine();
// Subtraction
// 1. Using operator "-"
resultV = vectorX - vectorY;
Console.WriteLine(@"Subtract vectors using operator -. (result = X - Y)");
Console.WriteLine(resultV.ToString("#0.00\t", formatProvider));
Console.WriteLine();
// 2. Using Subtract method and getting result into different vector instance
resultV = vectorX.Subtract(vectorY);
Console.WriteLine(@"Subtract vectors using method Subtract. (result = X.Subtract(Y))");
Console.WriteLine(resultV.ToString("#0.00\t", formatProvider));
Console.WriteLine();
// 3. Using Subtract method and updating vector itself
vectorX.Subtract(vectorY, vectorX);
Console.WriteLine(@"Subtract vectors using method Subtract. (X.Subtract(Y, X))");
Console.WriteLine(vectorX.ToString("#0.00\t", formatProvider));
Console.WriteLine();
// Divide by scalar
// 1. Using Divide method and getting result into different vector instance
resultV = vectorX.Divide(3.0);
Console.WriteLine(@"Divide vector by scalar using method Divide. (result = A.Divide(3.0))");
Console.WriteLine(resultV.ToString("#0.00\t", formatProvider));
Console.WriteLine();
// 2. Using Divide method and updating vector itself
vectorX.Divide(3.0, vectorX);
Console.WriteLine(@"Divide vector by scalar using method Divide. (X.Divide(3.0, X))");
Console.WriteLine(vectorX.ToString("#0.00\t", formatProvider));
Console.WriteLine();
// Outer Product of two vectors
// 1. Using instanse method OuterProduct
var resultM = vectorX.OuterProduct(vectorY);
Console.WriteLine(@"Outer Product of two vectors using method OuterProduct. (X.OuterProduct(Y))");
Console.WriteLine(resultM.ToString("#0.00\t", formatProvider));
Console.WriteLine();
// 2. Using static method of the Vector class
resultM = Vector.OuterProduct(vectorX, vectorY);
Console.WriteLine(@"Outer Product of two vectors using method OuterProduct. (Vector.OuterProduct(X,Y))");
Console.WriteLine(resultM.ToString("#0.00\t", formatProvider));
Console.WriteLine();
}
/// <summary>
/// Construct an initial simplex, given starting guesses for the constants, and
/// initial step sizes for each dimension
/// </summary>
private static Vector<double>[] InitializeVertices(SimplexConstant[] simplexConstants)
{
int numDimensions = simplexConstants.Length;
Vector<double>[] vertices = new Vector<double>[numDimensions + 1];
// define one point of the simplex as the given initial guesses
Vector<double> p0 = new DenseVector(numDimensions);
for (int i = 0; i < numDimensions; i++)
{
p0[i] = simplexConstants[i].Value;
}
// now fill in the vertices, creating the additional points as:
// P(i) = P(0) + Scale(i) * UnitVector(i)
vertices[0] = p0;
for (int i = 0; i < numDimensions; i++)
{
double scale = simplexConstants[i].InitialPerturbation;
Vector unitVector = new DenseVector(numDimensions);
unitVector[i] = 1;
vertices[i + 1] = p0.Add(unitVector.Multiply(scale));
}
return vertices;
}
/// <summary>
/// Compute the centroid of all points except the worst
/// </summary>
/// <param name="vertices"></param>
/// <param name="errorProfile"></param>
/// <returns></returns>
private static Vector ComputeCentroid(Vector<double>[] vertices, ErrorProfile errorProfile)
{
int numVertices = vertices.Length;
// find the centroid of all points except the worst one
Vector centroid = new DenseVector(numVertices - 1);
for (int i = 0; i < numVertices; i++)
{
if (i != errorProfile.HighestIndex)
{
centroid = (Vector) centroid.Add(vertices[i]);
}
}
return (Vector) centroid.Multiply(1.0d / (numVertices - 1));
}
private void solveActuator2CartesianDisp(double[] adisp)
{
bool check = false;
DenseVector cartDisp = new DenseVector(6);
DenseVector newAct = new DenseVector(adisp);
DenseVector actError = (DenseVector)newAct.Subtract(actuatorDisp);
cartesianDisp.CopyTo(cartDisp);
int iterations = 0;
while (check == false)
{
List2String l2s = new List2String();
DenseMatrix JacobianMatrix = new DenseMatrix(6, 6);
for (int i = 0; i < 6; i++)
{
DenseVector DL_Dd = actuators[i].calcNewDiffs(cartDisp.Values);
JacobianMatrix.SetRow(i, DL_Dd);
}
DenseVector diffCart = (DenseVector)JacobianMatrix.LU().Solve(actError);
log.Debug("Cartesian differences " + l2s.ToString(diffCart.Values));
cartDisp = (DenseVector)cartDisp.Add(diffCart);
setCartesianDisp(cartDisp.Values);
log.Debug("New cartesian estimate " + this);
actError = (DenseVector)newAct.Subtract(actuatorDisp);
log.Debug("Actuator error " + l2s.ToString(actError.Values));
check = withinErrorWindow(actError);
if (iterations > 20)
{
check = true;
log.Error("Calculations for " + label + " won't converge with " + this);
}
iterations++;
}
}
private void actuatorForce2Cartesian()
{
DenseVector force = new DenseVector(3);
DenseVector moment = new DenseVector(3);
DenseVector translation = new DenseVector(3);
cartesianDisp.CopySubVectorTo(translation, 0, 0, 3);
for (int a = 0; a < 6; a++)
{
DenseVector unitVector = (DenseVector)actuators[a].getDirectionalVector();
unitVector = (DenseVector)unitVector.Normalize(2.0);
DenseVector forceArm = actuators[a].getForceArm(translation);
DenseVector newForce = (DenseVector)unitVector.Multiply(actuatorForce[a]);
force = (DenseVector)force.Add(newForce);
DenseVectorCrossProduct cx = new DenseVectorCrossProduct(forceArm);
DenseVector newMoment = (DenseVector)cx.crossProduct(unitVector);
newMoment = (DenseVector)newMoment.Multiply(actuatorForce[a]);
moment = (DenseVector)moment.Add(newMoment);
}
cartesianForce.SetSubVector(0, 3, force);
cartesianForce.SetSubVector(3, 3, moment);
}
/// <summary>
/// Centers data to have mean zero along axis 0. This is here because
/// nearly all linear models will want their data to be centered.
/// If sample_weight is not None, then the weighted mean of X and y
/// is zero, and not the mean itself
/// </summary>
/// <param name="x"></param>
/// <param name="y"></param>
/// <param name="fitIntercept"></param>
/// <param name="normalize"></param>
/// <param name="sampleWeight"></param>
internal static CenterDataResult CenterData(
Matrix<double> x,
Matrix<double> y,
bool fitIntercept,
bool normalize = false,
Vector<double> sampleWeight = null)
{
Vector<double> xMean;
Vector<double> yMean = new DenseVector(y.ColumnCount);
Vector<double> xStd;
if (fitIntercept)
{
if (x is SparseMatrix)
{
xMean = DenseVector.Create(x.ColumnCount, i => 0.0);
xStd = DenseVector.Create(x.ColumnCount, i => 1.0);
}
else
{
if (sampleWeight == null)
{
xMean = x.MeanOfEveryColumn();
}
else
{
xMean = x.MulColumnVector(sampleWeight).SumOfEveryColumn().Divide(sampleWeight.Sum());
}
x = x.SubtractRowVector(xMean);
if (normalize)
{
xStd = new DenseVector(x.ColumnCount);
foreach (var row in x.RowEnumerator())
{
xStd.Add(row.Item2.PointwiseMultiply(row.Item2), xStd);
}
xStd.MapInplace(Math.Sqrt);
for (int i = 0; i < xStd.Count; i++)
{
if (xStd[i] == 0)
{
xStd[i] = 1;
}
}
x.DivRowVector(xStd, x);
}
else
{
xStd = DenseVector.Create(x.ColumnCount, i => 1.0);
}
}
if (sampleWeight == null)
{
yMean = y.MeanOfEveryColumn();
}
else
{
yMean = y.MulColumnVector(sampleWeight).SumOfEveryColumn() / sampleWeight.Sum();
}
y = y.Clone();
y = y.SubtractRowVector(yMean);
}
else
{
xMean = DenseVector.Create(x.ColumnCount, i => 0);
xStd = DenseVector.Create(x.ColumnCount, i => 1);
}
return new CenterDataResult { X = x, Y = y, xMean = xMean, yMean = yMean, xStd = xStd };
}
