private Matrix <double> Equal(Matrix <double> z)
{
var x = z.SubMatrix(0, n, 0, 1);
var u = z.SubMatrix(n, p, 0, 1);
var dx = problem.SystemDynamicFunction(x, u);
var y = problem.ObservationFunction();
var hx = Filter(x, x0, xConstrain);
var hu = Filter(u, u0, uConstrain);
var hy = Filter(y, y0, yConstrain);
var hdx = Filter(dx, dx0, dxConstrain);
return(SignalUtils.Stack(hx, hu, hy, hdx));
}
public void Linearize(Problem problem)
{
modelName = problem.Name;
var xStar = State.StatePool.ConvertAll(state => state.StateVariable).ToColumnVector();
var uStar = ControlInput.InputPool.ConvertAll(input => input.Input).ToColumnVector();
var dx = 0.1 * xStar.PointwiseAbs() + 1;
var du = 0.1 * uStar.PointwiseAbs() + 1;
var f0 = problem.SystemDynamicFunction(xStar, uStar);
var g0 = problem.ObservationFunction();
var build = Matrix <double> .Build;
A = build.DenseOfColumnVectors(xStar.Column(0).Select((row, index) =>
{
return(Algorithm.PrecisePartial(d =>
{
return problem.SystemDynamicFunction(xStar + Mask(xStar.RowCount, index, d), uStar) - f0;
}, dx[index, 0]));
}).Select((mat, i) => mat.Column(0)));
B = build.DenseOfColumnVectors(uStar.Column(0).Select((row, index) =>
{
return(Algorithm.PrecisePartial(d =>
{
return problem.SystemDynamicFunction(xStar, uStar + Mask(uStar.RowCount, index, d)) - f0;
}, du[index, 0]));
}).Select((mat, i) => mat.Column(0)));
C = build.DenseOfColumnVectors(xStar.Column(0).Select((row, index) =>
{
return(Algorithm.PrecisePartial(d =>
{
problem.SystemDynamicFunction(xStar + Mask(xStar.RowCount, index, d), uStar);
return problem.ObservationFunction() - g0;
}, dx[index, 0]));
}).Select((mat, i) => mat.Column(0)));
D = build.DenseOfColumnVectors(uStar.Column(0).Select((row, index) =>
{
return(Algorithm.PrecisePartial(d =>
{
problem.SystemDynamicFunction(xStar, uStar + Mask(uStar.RowCount, index, d));
return problem.ObservationFunction() - g0;
}, du[index, 0]));
}).Select((mat, i) => mat.Column(0)));
}