//Assumes full observability and no D matrix.
//Generates an empty set of matrices representing the state-space model.
public LinearStateSpaceModel(int numberOfStates = 6, int numberOfInputs = 2)
{
A = CreateMatrix.Dense <double>(numberOfStates, numberOfStates);
B = CreateMatrix.Dense <double>(numberOfStates, numberOfInputs);
C = CreateMatrix.DenseIdentity <double>(numberOfStates);
D = CreateMatrix.Dense <double>(0, 0);
}
protected override Vector <double> CalculateSearchDirection(ref Matrix <double> inversePseudoHessian,
out double maxLineSearchStep,
out double startingStepSize,
IObjectiveFunction previousPoint,
IObjectiveFunction candidate,
Vector <double> step)
{
startingStepSize = 1.0;
maxLineSearchStep = double.PositiveInfinity;
Vector <double> lineSearchDirection;
var y = candidate.Gradient - previousPoint.Gradient;
double sy = step * y;
inversePseudoHessian = inversePseudoHessian + ((sy + y * inversePseudoHessian * y) / Math.Pow(sy, 2.0)) * step.OuterProduct(step) - ((inversePseudoHessian * y.ToColumnMatrix()) * step.ToRowMatrix() + step.ToColumnMatrix() * (y.ToRowMatrix() * inversePseudoHessian)) * (1.0 / sy);
lineSearchDirection = -inversePseudoHessian * candidate.Gradient;
if (lineSearchDirection * candidate.Gradient >= 0.0)
{
lineSearchDirection = -candidate.Gradient;
inversePseudoHessian = CreateMatrix.DenseIdentity <double>(candidate.Point.Count);
}
return(lineSearchDirection);
}
public void GetGlobalKeepMatrix()
{
int NumberOfActiveDOF = 0;
int NumberOfAvailableDOF = 0;
int RowIndex;
foreach (Element i in Elements)
{
NumberOfActiveDOF += Convert.ToInt32(GetElementStateExistanceVector(i.ElementId).Sum()) / 2;
NumberOfAvailableDOF += 6;
}
Matrix <double> GlobalKeepMatrix = CreateMatrix.Dense <double>(NumberOfActiveDOF * 3, NumberOfAvailableDOF * 3);
foreach (Element i in Elements)
{
Vector <double> DOFExist = GetElementStateExistanceVector(i.ElementId);
RowIndex = 0;
for (int index = 0; index < 6; index++)
{
if (DOFExist[index] == 1)
{
GlobalKeepMatrix.InsertAtIndex(CreateMatrix.DenseIdentity <double>(3), index * 3, RowIndex);
RowIndex += 3;
}
}
}
this.KeepMatrix = GlobalKeepMatrix;
}
public void Train(Matrix <double> pattern)
{
if (pattern.RowCount != Dimension || pattern.ColumnCount != Dimension)
{
throw new Exception("Incompatible image dimensions");
}
if (PatternCount >= MaxPatternCount)
{
throw new Exception("Full Memory");
}
var tempMatrix = CreateMatrix.Dense <double>(NeuronsCount, NeuronsCount);
// convert 0 => -1
pattern.MapInplace(x => x == 0 ? -1 : x, Zeros.Include);
// to column vector
var col = pattern.AsColumnMajorArray();
var columnVector = CreateMatrix.DenseOfColumnArrays(col);
// create weighted matrix
columnVector.TransposeAndMultiply(columnVector, tempMatrix);
//set diagonal to 0
tempMatrix = tempMatrix.Subtract(CreateMatrix.DenseIdentity <double>(NeuronsCount, NeuronsCount));
// add pattern
Memory = Memory + tempMatrix;
PatternCount++;
}
/// <summary>
/// Calculates the local mass matrix
/// </summary>
/// <returns>The local mass matrix</returns>
private Matrix <double> GetLocalMassMatrix()
{
Matrix <double> LocalMassMatrix = CreateMatrix.Dense <double>(6, 6);
LocalMassMatrix.InsertAtIndex(CreateMatrix.DenseIdentity <double>(3) * this.Mass, 0);
LocalMassMatrix.InsertAtIndex(this.Inertia, 3);
return(LocalMassMatrix);
}
/// <summary>
/// Calculates the derivative of the "Local Matrix", see 'GetLocalMatrix'
/// </summary>
/// <returns>Matrix with rotational matrices on its diagonal</returns>
private Matrix <double> GetLocalMatrixDerivative()
{
int NumElements = System.Elements.Count;
int ElementIndex = GetElementIndex() * 3;
Matrix <double> LocalMatrixRotationDerivative = CreateMatrix.Dense <double>(NumElements * 6 * 3, NumElements * 6 * 3);
for (int i = 0; i < LocalMatrixRotationDerivative.ColumnCount; i += 3)
{
LocalMatrixRotationDerivative.InsertAtIndex(this.LocalRotationMatrixTotalDerivative, i);
}
LocalMatrixRotationDerivative.InsertAtIndex(LocalRotationMatrixPartialDiffTotalGamma * LocalRotationMatrixPartialDiffBeta, ElementIndex + 3 * 3);
LocalMatrixRotationDerivative.InsertAtIndex(LocalRotationMatrixPartialDiffBeta, ElementIndex + 3 * 3 + 3);
LocalMatrixRotationDerivative.InsertAtIndex(CreateMatrix.DenseIdentity <double>(3), ElementIndex + 3 * 3 + 6);
return(LocalMatrixRotationDerivative);
}
public void IterateDLS(Hinge joint, Vector2 goal)
{
var jacobian = ComputeJacobian(joint);
var deltaE = ClampDeltaE(goal, joint.EndEffector);
var jacobianMat = CreateMatrix.Dense(2, jacobian.Count, (row, col) => row == 0 ? jacobian[col].X : jacobian[col].Y);
var lambda = DLSLambda;
var lambdaMatrix = lambda * lambda * CreateMatrix.DenseIdentity <float>(jacobianMat.RowCount);
var deltaPhiVec =
jacobianMat.Transpose() *
(jacobianMat * jacobianMat.Transpose() + (lambdaMatrix)).Inverse() *
CreateVector.DenseOfArray(new float[] { deltaE.X, deltaE.Y });
var deltaPhi = new LinkedList <float>(deltaPhiVec.ToArray());
joint.ApplyDofDeltas(deltaPhi);
}
private void DLSIterate()
{
var jacobian = ComputeJacobian();
var deltaE = SafeDeltaE(goal, baseJoint.EndEffector);
var jacobianMat = CreateMatrix.Dense(2, jacobian.Count, (row, col) => row == 0 ? jacobian[col].X : jacobian[col].Y);
var lambda = .5f;
var lambdaMatrix = lambda * lambda * CreateMatrix.DenseIdentity <float>(jacobianMat.RowCount);
var deltaPhiVec =
jacobianMat.Transpose() *
(jacobianMat * jacobianMat.Transpose() + (lambdaMatrix)).Inverse() *
CreateVector.DenseOfArray(new float[] { deltaE.X, deltaE.Y });
var deltaPhi = new LinkedList <float>(deltaPhiVec.ToArray());
baseJoint.ApplyDofDeltas(deltaPhi);
}
/// <summary>
/// Calculates a matrix with the local rotation Matrix on its diagonal and gamma * beta, beta and identity on the elements rotational degrees of freedom
/// </summary>
/// <returns></returns>
private Matrix <double> GetLocalMatrixRotation()
{
int NumElements = System.Elements.Count;
int ElementIndex = GetElementIndex() * 3;
Matrix <double> LocalMatrixRotation = CreateMatrix.DenseIdentity <double>(NumElements * 6 * 3, NumElements * 6 * 3);
for (int i = ElementIndex + 3 * 6; i < LocalMatrixRotation.ColumnCount; i += 3 + 6 * 3)
{
LocalMatrixRotation.InsertAtIndex(LocalRotationMatrix, i);
}
LocalMatrixRotation.InsertAtIndex(CreateMatrix.DenseIdentity <double>(9), ElementIndex);
LocalMatrixRotation.InsertAtIndex(this.LocalRotationMatrixGamma * this.LocalRotationMatrixBeta, ElementIndex + 3 * 3);
LocalMatrixRotation.InsertAtIndex(this.LocalRotationMatrixBeta, ElementIndex + 3 * 3 + 3);
LocalMatrixRotation.InsertAtIndex(CreateMatrix.DenseIdentity <double>(3), ElementIndex + 3 * 3 + 6);
return(LocalMatrixRotation);
}
/// <summary>
/// Calculates the "Local Matrix" (matrix with the partial diff rotation matrix and a unity matrix on the local states and the element rotation matrix on the others)
/// </summary>
/// <param name="LocalStateVector">Vector of the local states</param>
/// <returns>Matrix with Rotational matrices on its diagonal</returns>
private Matrix <double> GetLocalMatrixTranslation()
{
int NumElements = System.Elements.Count;
int ElementIndex = GetElementIndex() * 3;
Matrix <double> LocalMatrixTranslation = CreateMatrix.Dense <double>(NumElements * 6 * 3, NumElements * 6 * 3);
for (int i = 0; i < LocalMatrixTranslation.ColumnCount; i += 3)
{
LocalMatrixTranslation.InsertAtIndex(LocalRotationMatrix * 2, i);
}
LocalMatrixTranslation.InsertAtIndex(CreateMatrix.DenseIdentity <double>(9), ElementIndex);
LocalMatrixTranslation.InsertAtIndex(LocalRotationMatrixPartialDiffAlpha, ElementIndex + 3 * 3);
LocalMatrixTranslation.InsertAtIndex(LocalRotationMatrixPartialDiffBeta * 3, ElementIndex + 3 * 3 + 3);
LocalMatrixTranslation.InsertAtIndex(LocalRotationMatrixPartialDiffGamma * 4, ElementIndex + 3 * 3 + 6);
return(LocalMatrixTranslation);
}
/// <summary>
/// Find the minimum of the objective function given lower and upper bounds
/// </summary>
/// <param name="objective">The objective function, must support a gradient</param>
/// <param name="initialGuess">The initial guess</param>
/// <returns>The MinimizationResult which contains the minimum and the ExitCondition</returns>
public MinimizationResult FindMinimum(IObjectiveFunction objective, Vector <double> initialGuess)
{
if (!objective.IsGradientSupported)
{
throw new IncompatibleObjectiveException("Gradient not supported in objective function, but required for BFGS minimization.");
}
objective.EvaluateAt(initialGuess);
ValidateGradientAndObjective(objective);
// Check that we're not already done
ExitCondition currentExitCondition = ExitCriteriaSatisfied(objective, null, 0);
if (currentExitCondition != ExitCondition.None)
{
return(new MinimizationResult(objective, 0, currentExitCondition));
}
// Set up line search algorithm
var lineSearcher = new WeakWolfeLineSearch(1e-4, 0.9, Math.Max(ParameterTolerance, 1e-10), 1000);
// First step
var inversePseudoHessian = CreateMatrix.DenseIdentity <double>(initialGuess.Count);
var lineSearchDirection = -objective.Gradient;
var stepSize = 100 * GradientTolerance / (lineSearchDirection * lineSearchDirection);
var previousPoint = objective;
LineSearchResult lineSearchResult;
try
{
lineSearchResult = lineSearcher.FindConformingStep(objective, lineSearchDirection, stepSize);
}
catch (OptimizationException e)
{
throw new InnerOptimizationException("Line search failed.", e);
}
catch (ArgumentException e)
{
throw new InnerOptimizationException("Line search failed.", e);
}
var candidate = lineSearchResult.FunctionInfoAtMinimum;
ValidateGradientAndObjective(candidate);
var gradient = candidate.Gradient;
var step = candidate.Point - initialGuess;
// Subsequent steps
Matrix <double> I = CreateMatrix.DiagonalIdentity <double>(initialGuess.Count);
int iterations;
int totalLineSearchSteps = lineSearchResult.Iterations;
int iterationsWithNontrivialLineSearch = lineSearchResult.Iterations > 0 ? 0 : 1;
iterations = DoBfgsUpdate(ref currentExitCondition, lineSearcher, ref inversePseudoHessian, ref lineSearchDirection, ref previousPoint, ref lineSearchResult, ref candidate, ref step, ref totalLineSearchSteps, ref iterationsWithNontrivialLineSearch);
if (iterations == MaximumIterations && currentExitCondition == ExitCondition.None)
{
throw new MaximumIterationsException(String.Format("Maximum iterations ({0}) reached.", MaximumIterations));
}
return(new MinimizationWithLineSearchResult(candidate, iterations, ExitCondition.AbsoluteGradient, totalLineSearchSteps, iterationsWithNontrivialLineSearch));
}
public Matrix <double> Resolve(Parameter parameter) => CreateMatrix.DenseIdentity <double>(size);