//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);