public Matrix <double> p0()
{
double th = 0.000000001;
Matrix <double> L = Lambda;
L.CoerceZero(th);
Vector <double> Lcs = L.ColumnSums();
int nonZeroLcsCount = 0;
int lastNonZero = -1;
for (int i = 0; i < Lcs.Count(); i++)
{
if (Math.Abs(Lcs[i]) < th)
{
L[i, i] = 1.0;
}
else
{
nonZeroLcsCount++;
lastNonZero = i;
}
}
if (nonZeroLcsCount == 1)
{
return(VBuilder.Dense(N, i => i == lastNonZero ? 1 : 0).ToColumnMatrix());
}
else
{
Matrix <double> A = MBuilder.Dense(1, N, 1.0);
A = A.Stack(Lambda.Transpose());
Vector <double> B = VBuilder.Dense(N + 1, i => i > 0 ? 0 : 1);
Vector <double> result = A.Solve(B);
return(result.ToColumnMatrix());
}
}
public void CalculateObservation()
{
// 位置控制环
F1 = vb.Dense(new[] {
_plane.Vk *(Cos(_plane.Gamma) * Sin(_plane.Chi) - _plane.Chi),
-_plane.Vk * (Sin(_plane.Gamma) - _plane.Gamma)
});
B1 = mb.DenseOfDiagonalArray(new double[] { _plane.Vk, -_plane.Vk });
}
public Layer(int nodeCount)
{
this.NodeCount = nodeCount;
// All layers have a bias node, even the output (can ignore)
this.Activations = V.Dense(nodeCount + 1);
this.Activations[0] = 1;
this.RawValues = V.Dense(nodeCount);
this.Gradients = V.Dense(nodeCount);
this.Deltas = V.Dense(nodeCount);
}
// event RecordShipStateEvent;
public Ship()
{
DeckEnable = Configuration.IsDeckCompensationEnabled;
omega_d_2i = vb.Dense(new double[] { omega_dx_2i, omega_dy_2i, omega_dz_2i });
DeckPosition = Position;
if (DeckEnable)
{
ForwardFilterState = MatlabReader.Read <double>("./ForwardFilter.mat", "forward_filter_state");
CurrentDeckPredict = MatlabReader.Read <double>("./ForwardFilter.mat", "current_deck_predict").Column(0);
CurrentDeckControl = MatlabReader.Read <double>("./ForwardFilter.mat", "current_deck_control").Column(0);
CurrentDeckLateralControl = MatlabReader.Read <double>("./ForwardFilter.mat", "current_deck_control_lat").Column(0);
}
}
public static Vector <double> Cross(Vector <double> left, Vector <double> right)
{
if ((left.Count != 3 || right.Count != 3))
{
string message = "Vectors must have a length of 3.";
throw new Exception(message);
}
Vector <double> result = v_builder.Dense(3);
result[0] = left[1] * right[2] - left[2] * right[1];
result[1] = -left[0] * right[2] + left[2] * right[0];
result[2] = left[0] * right[1] - left[1] * right[0];
return(result);
}
static Vector <double> MonteKarlo(Matrix <double> A, Vector <double> b, int N, int M)
{
int n = b.Count();
Matrix <double> a = MatrixBuilder.Dense(n, n, (i, j) => i == j ? 0 : -A[i, j] / A[i, i]);
Vector <double> f = VectorBuilder.Dense(n, i => b[i] / A[i, i]);
Vector <double> x = VectorBuilder.Dense(n);
Vector <double> h = VectorBuilder.Dense(n);
Vector <double> pi = VectorBuilder.Dense(n, 1.0 / 3);
Matrix <double> P = MatrixBuilder.Dense(n, n, (i, j) => 1.0 / 3);
double[] E = new double[M];
int[] MarkovСhain = new int[N + 1];
double[] Weights = new double[N + 1];
for (int i = 0; i < n; i++)
{
h.Clear();
h[i] = 1;
Array.Clear(E, 0, E.Length);
for (int j = 0; j < M; j++)
{
for (int k = 0; k <= N; k++)
{
MarkovСhain[k] = random.Next(0, 3);
}
Weights[0] = pi[MarkovСhain[0]] > 0 ? h[MarkovСhain[0]] / pi[MarkovСhain[0]] : 0;
for (int k = 1; k <= N; k++)
{
Weights[k] = P[MarkovСhain[k - 1], MarkovСhain[k]] > 0 ? Weights[k - 1] * a[MarkovСhain[k - 1], MarkovСhain[k]] / P[MarkovСhain[k - 1], MarkovСhain[k]] : 0;
}
for (int k = 0; k <= N; k++)
{
E[j] += Weights[k] * f[MarkovСhain[k]];
}
}
x[i] = E.Sum() / M;
}
return(x);
}
static void Main(string[] args)
{
Matrix <double> w0 = M.Random(InputNodes, HiddenNodes, new ContinuousUniform());
Matrix <double> w1 = M.Random(HiddenNodes, OutputNodes, new ContinuousUniform());
int iterCount = 0;
for (int i = 0; i < Iterations; i++)
{
iterCount++;
for (int j = 0; j < TestData.RowCount; j++)
{
//Forwards Propagation
Vector <double> l0 = TestData.Row(j);
Vector <double> l1 = w0.LeftMultiply(l0).Map(Relu);
Vector <double> l2 = w1.LeftMultiply(l1);
double goal = Correct[j];
double prediction = l2.Sum();
double error = Math.Pow(goal - prediction, 2);
LogPrediction(prediction, error);
//Backward Propagation
//Calculate delta at each layer
Vector <double> l2Delta = V.Dense(1, goal - prediction);
Vector <double> l1Delta = w1.Transpose().LeftMultiply(l2Delta).Map2((a, b) => a * Relu2Deriv(b), l1);
//Update Weights
w0 = w0.Add(l1Delta.ToRowMatrix().StackSelf(InputNodes).PointwiseMultiply(l0.ToRowMatrix().StackSelf(HiddenNodes).Transpose()).Multiply(Alpha));
w1 = w1.Add(l1.Multiply(l2Delta.Sum()).Multiply(Alpha).ToColumnMatrix());
}
}
Console.ReadLine();
}
public static Vector <Complex32> Vector(TestVector vector)
{
switch (vector)
{
case TestVector.Dense5: return(V.Dense(new[] { new Complex32(1, 1), new Complex32(2, 1), new Complex32(3, 1), new Complex32(4, 1), new Complex32(5, 1) }));
case TestVector.Dense5WithZeros: return(V.Dense(new[] { new Complex32(2, -1), new Complex32(0, 0), new Complex32(0, 2), new Complex32(-5, 1), new Complex32(0, 0) }));
case TestVector.Sparse5: return(V.SparseOfEnumerable(new[] { new Complex32(1, 1), new Complex32(2, 1), new Complex32(3, 1), new Complex32(4, 1), new Complex32(5, 1) }));
case TestVector.Sparse5WithZeros: return(V.SparseOfEnumerable(new[] { new Complex32(2, -1), new Complex32(0, 0), new Complex32(0, 2), new Complex32(-5, 1), new Complex32(0, 0) }));
case TestVector.Sparse5AllZeros: return(V.Sparse(5));
case TestVector.SparseMaxLengthAllZeros: return(V.Sparse(int.MaxValue));
default: throw new NotSupportedException();
}
}
public static Vector <float> Vector(TestVector vector)
{
switch (vector)
{
case TestVector.Dense5: return(V.Dense(new float[] { 1, 2, 3, 4, 5 }));
case TestVector.Dense5WithZeros: return(V.Dense(new float[] { 2, 0, 0, -5, 0 }));
case TestVector.Sparse5: return(V.SparseOfEnumerable(new float[] { 1, 2, 3, 4, 5 }));
case TestVector.Sparse5WithZeros: return(V.SparseOfEnumerable(new float[] { 2, 0, 0, -5, 0 }));
case TestVector.Sparse5AllZeros: return(V.Sparse(5));
case TestVector.SparseMaxLengthAllZeros: return(V.Sparse(int.MaxValue));
default: throw new NotSupportedException();
}
}
public Model(int NumberOfJoints, int NumberOfSupports, int NumberOfMaterials, int NumberOfSections, int NumberOfMembers, int NumberOfLoads)
{
JointCoordinates = M.Dense(NumberOfJoints, 2);
SupportData = M.Dense(NumberOfSupports, 3);
MemberData = M.Dense(NumberOfMembers, 4);
LoadData = M.Dense(NumberOfLoads, 2);
ElasticModulus = V.Dense(NumberOfMaterials);
CrossSectionalArea = V.Dense(NumberOfSections);
JointLoads = V.Dense(NumberOfLoads);
}
public L1Adaptive(Ship ship, Plane plane, IControlModule module)
{
Ship = ship;
Plane = plane;
ControlModule = module;
k_pr = mb.DenseOfDiagonalArray(new[] { k_p, k_r });
Amq = -1 / T_pitch;
w_cq = 1 / T_cq;
Asq = Amq + ksq;
Amp = -1 / T_roll;
w_cp = 1 / T_cp;
Amr = -1 / T_yaw;
w_cr = 1 / T_cr;
pr_ad_dot = vb.Dense(new[] { p_ad_dot, r_ad_dot });
}
//event RecordAttitudeLoopEvent;
public AttitudeLoop(Plane plane, Ship ship)
{
this.plane = plane;
this.ship = ship;
U2 = vb.Dense(new[] { plane.DesiredParameter.Alpha, 0, 0 });
U3FilterBuffer = mb.Dense(sampleNumber, 3, 0); // p q r
X3 = vb.Dense(new[]
{ plane.Alpha, plane.Beta, plane.Miu });
previousU3 = U3;
filteredU2 = U2;
//addlistener(plane, 'X3ChangedEvent', @updateState);
}
// Use this for initialization
void Start()
{
/*SparseMatrix A = SparseMatrix.OfRowArrays(new double[7][]
* {
* new double[] {0, -1, -1, -1, -1, -1, -1},
* new double[] {-1, 1, 0, 0, 0, 0, 0},
* new double[] {-1, 0, 1, 0, 0, 0, 0},
* new double[] {-1, 0, 0, 1, 0, 0, 0},
* new double[] {-1, 0, 0, 0, 1, 0, 0},
* new double[] {-1, 0, 0, 0, 0, 1, 0},
* new double[] {-1, 0, 1, 0, 0, 0, 1}
* });*/
Func <int, int, double> init = cValues;
SparseMatrix A = SparseMatrix.Create(7, 7, init);
double lambda = -993f / (6f * 0.01f);
lambda *= 1f - 5f / 7f;
double nDefault = 5f;
VectorBuilder <double> Builder = Vector <double> .Build;
Vector <double> B = Builder.DenseOfArray(new double[7]
{
0,
lambda,
lambda,
lambda,
lambda,
lambda,
lambda,
});
VectorBuilder <double> resultB = Vector <double> .Build;
Vector <double> result = resultB.Dense(7);
A.Solve(B, result);
Debug.Log("Matrix A is : " + A.ToString());
Debug.Log("Matrix B is : " + B.ToString());
Debug.Log("Lambda is : " + lambda);
Debug.Log("The result vector3 is : " + result.ToString());
}
private static DoubleCurve <T> Build(List <Contract <T> > contracts, Func <T, double> weighting,
Func <T, double> multAdjustFunc, Func <T, double> addAdjustFunc, Func <T, T, double> timeFunc,
double?frontFirstDerivative, double?backFirstDerivative)
{
if (contracts.Count < 2)
{
throw new ArgumentException("contracts must have at least two elements", nameof(contracts));
}
var curveStartPeriod = contracts[0].Start;
int numGaps = 0;
var timeToPolynomialBoundaries = new List <double>();
// TODO optionally do/don't allow gaps in contracts
for (int i = 0; i < contracts.Count - 1; i++)
{
var contractEnd = contracts[i].End;
var nextContractStart = contracts[i + 1].Start;
if (contractEnd.CompareTo(nextContractStart) >= 0)
{
throw new ArgumentException("contracts are overlapping");
}
timeToPolynomialBoundaries.Add(timeFunc(curveStartPeriod, nextContractStart));
if (contractEnd.OffsetFrom(nextContractStart) < -1) // Gap in contracts
{
numGaps++;
timeToPolynomialBoundaries.Add(timeFunc(curveStartPeriod, contractEnd.Next()));
}
}
int numPolynomials = contracts.Count + numGaps;
int numCoefficientsToSolve = numPolynomials * 5;
int numConstraints =
(numPolynomials - 1) * 3 // Spline value, 1st derivative, and 2nd derivative constraints
+ numPolynomials - numGaps // Price constraints
+ (frontFirstDerivative.HasValue ? 1 : 0)
+ (backFirstDerivative.HasValue ? 1 : 0);
MatrixBuilder <double> matrixBuilder = Matrix <double> .Build;
VectorBuilder <double> vectorBuilder = Vector <double> .Build;
var constraintMatrix = matrixBuilder.Dense(numConstraints, numCoefficientsToSolve);
var vector = vectorBuilder.Dense(numPolynomials * 5 + numConstraints);
var twoHMatrix = matrixBuilder.Dense(numPolynomials * 5, numPolynomials * 5);
int inputContractIndex = 0;
bool gapFilled = false;
int rowNum = 0;
for (int i = 0; i < numPolynomials; i++)
{
int colOffset = i * 5;
if (i < numPolynomials - 1)
{
double timeToPolynomialBoundary = timeToPolynomialBoundaries[i];
double timeToPolynomialBoundaryPow2 = Math.Pow(timeToPolynomialBoundary, 2);
double timeToPolynomialBoundaryPow3 = Math.Pow(timeToPolynomialBoundary, 3);
double timeToPolynomialBoundaryPow4 = Math.Pow(timeToPolynomialBoundary, 4);
// Polynomial equality at boundaries
constraintMatrix[rowNum, colOffset] = 1.0;
constraintMatrix[rowNum, colOffset + 1] = timeToPolynomialBoundary;
constraintMatrix[rowNum, colOffset + 2] = timeToPolynomialBoundaryPow2;
constraintMatrix[rowNum, colOffset + 3] = timeToPolynomialBoundaryPow3;
constraintMatrix[rowNum, colOffset + 4] = timeToPolynomialBoundaryPow4;
constraintMatrix[rowNum, colOffset + 5] = -1.0;
constraintMatrix[rowNum, colOffset + 6] = -timeToPolynomialBoundary;
constraintMatrix[rowNum, colOffset + 7] = -timeToPolynomialBoundaryPow2;
constraintMatrix[rowNum, colOffset + 8] = -timeToPolynomialBoundaryPow3;
constraintMatrix[rowNum, colOffset + 9] = -timeToPolynomialBoundaryPow4;
// Polynomial first derivative equality at boundaries
constraintMatrix[rowNum + 1, colOffset] = 0.0;
constraintMatrix[rowNum + 1, colOffset + 1] = 1.0;
constraintMatrix[rowNum + 1, colOffset + 2] = 2.0 * timeToPolynomialBoundary;
constraintMatrix[rowNum + 1, colOffset + 3] = 3.0 * timeToPolynomialBoundaryPow2;
constraintMatrix[rowNum + 1, colOffset + 4] = 4.0 * timeToPolynomialBoundaryPow3;
constraintMatrix[rowNum + 1, colOffset + 5] = 0.0;
constraintMatrix[rowNum + 1, colOffset + 6] = -1.0;
constraintMatrix[rowNum + 1, colOffset + 7] = -2.0 * timeToPolynomialBoundary;
constraintMatrix[rowNum + 1, colOffset + 8] = -3.0 * timeToPolynomialBoundaryPow2;
constraintMatrix[rowNum + 1, colOffset + 9] = -4.0 * timeToPolynomialBoundaryPow3;
// Polynomial second derivative equality at boundaries
constraintMatrix[rowNum + 2, colOffset] = 0.0;
constraintMatrix[rowNum + 2, colOffset + 1] = 0.0;
constraintMatrix[rowNum + 2, colOffset + 2] = 2.0;
constraintMatrix[rowNum + 2, colOffset + 3] = 6.0 * timeToPolynomialBoundary;
constraintMatrix[rowNum + 2, colOffset + 4] = 12.0 * timeToPolynomialBoundaryPow2;
constraintMatrix[rowNum + 2, colOffset + 5] = 0.0;
constraintMatrix[rowNum + 2, colOffset + 6] = 0.0;
constraintMatrix[rowNum + 2, colOffset + 7] = -2.0;
constraintMatrix[rowNum + 2, colOffset + 8] = -6 * timeToPolynomialBoundary;
constraintMatrix[rowNum + 2, colOffset + 9] = -12.0 * timeToPolynomialBoundaryPow2;
}
// Contract price constraint
if (i == 0 || // Can't be gap at the first position
contracts[inputContractIndex - 1].End.OffsetFrom(contracts[inputContractIndex].Start) ==
-1 || // No gap from previous
gapFilled) // Gap has already been dealt with
{
Contract <T> contract = contracts[inputContractIndex];
double sumWeight = 0.0;
double sumWeightMult = 0.0;
double sumWeightMultTime = 0.0;
double sumWeightMultTimePow2 = 0.0;
double sumWeightMultTimePow3 = 0.0;
double sumWeightMultTimePow4 = 0.0;
double sumWeightMultAdd = 0.0;
foreach (T timePeriod in contract.Start.EnumerateTo(contract.End))
{
double timeToPeriod = timeFunc(curveStartPeriod, timePeriod);
double weight = weighting(timePeriod);
double multAdjust = multAdjustFunc(timePeriod);
double addAdjust = addAdjustFunc(timePeriod);
sumWeight += weight;
sumWeightMult += weight * multAdjust;
sumWeightMultTime += weight * multAdjust * timeToPeriod;
sumWeightMultTimePow2 += weight * multAdjust * Math.Pow(timeToPeriod, 2.0);
sumWeightMultTimePow3 += weight * multAdjust * Math.Pow(timeToPeriod, 3.0);
sumWeightMultTimePow4 += weight * multAdjust * Math.Pow(timeToPeriod, 4.0);
sumWeightMultAdd += weight * multAdjust * addAdjust;
}
int priceConstraintRow = i == (numPolynomials - 1) ? rowNum : rowNum + 3;
constraintMatrix[priceConstraintRow, colOffset] = sumWeightMult; // Coefficient of a
constraintMatrix[priceConstraintRow, colOffset + 1] = sumWeightMultTime; // Coefficient of b
constraintMatrix[priceConstraintRow, colOffset + 2] = sumWeightMultTimePow2; // Coefficient of c
constraintMatrix[priceConstraintRow, colOffset + 3] = sumWeightMultTimePow3; // Coefficient of d
constraintMatrix[priceConstraintRow, colOffset + 4] = sumWeightMultTimePow4; // Coefficient of e
vector[numPolynomials * 5 + priceConstraintRow] = sumWeight * contract.Price - sumWeightMultAdd;
twoHMatrix.SetSubMatrix(i * 5 + 2, i * 5 + 2,
Create2HBottomRightSubMatrix(contract, curveStartPeriod, timeFunc));
inputContractIndex++;
rowNum += 4;
gapFilled = false;
}
else
{
// Gap in contracts
rowNum += 3;
gapFilled = true;
}
}
// TODO unit test first derivative constraints. How?
rowNum -= 3;
if (frontFirstDerivative.HasValue)
{
constraintMatrix[rowNum, 1] = 1; // Coefficient of b
vector[numPolynomials * 5 + rowNum] = frontFirstDerivative.Value;
rowNum++;
}
if (backFirstDerivative.HasValue)
{
T lastPeriod = contracts[contracts.Count - 1].End;
double timeToEnd = timeFunc(curveStartPeriod, lastPeriod.Offset(1));
constraintMatrix[rowNum, numCoefficientsToSolve - 4] = 1; // Coefficient of b
constraintMatrix[rowNum, numCoefficientsToSolve - 3] = 2 * timeToEnd; // Coefficient of c
constraintMatrix[rowNum, numCoefficientsToSolve - 2] = 3 * Math.Pow(timeToEnd, 2); // Coefficient of d
constraintMatrix[rowNum, numCoefficientsToSolve - 1] = 4 * Math.Pow(timeToEnd, 3); // Coefficient of e
vector[numPolynomials * 5 + rowNum] = backFirstDerivative.Value;
}
// Create system of equations to solve
Matrix <double> tempMatrix1 = twoHMatrix.Append(constraintMatrix.Transpose());
var tempMatrix2 = constraintMatrix.Append(
matrixBuilder.Dense(constraintMatrix.RowCount, constraintMatrix.RowCount));
var matrix = tempMatrix1.Stack(tempMatrix2);
Vector <double> solution = matrix.Solve(vector);
// Read off results from polynomial
T curveEndPeriod = contracts[contracts.Count - 1].End;
int numOutputPeriods = curveEndPeriod.OffsetFrom(curveStartPeriod) + 1;
var outputCurvePeriods = new T[numOutputPeriods];
var outputCurvePrices = new double[numOutputPeriods];
int outputContractIndex = 0;
gapFilled = false;
inputContractIndex = 0;
for (int i = 0; i < numPolynomials; i++)
{
double EvaluateSpline(T timePeriod)
{
double timeToPeriod = timeFunc(curveStartPeriod, timePeriod);
int solutionOffset = i * 5;
double splineValue = solution[solutionOffset] +
solution[solutionOffset + 1] * timeToPeriod +
solution[solutionOffset + 2] * Math.Pow(timeToPeriod, 2) +
solution[solutionOffset + 3] * Math.Pow(timeToPeriod, 3) +
solution[solutionOffset + 4] * Math.Pow(timeToPeriod, 4);
double multAdjust = multAdjustFunc(timePeriod);
double addAdjust = addAdjustFunc(timePeriod);
return((splineValue + addAdjust) * multAdjust);
}
T start;
T end;
if (i == 0 || // Can't be gap at the first position
contracts[inputContractIndex - 1].End.OffsetFrom(contracts[inputContractIndex].Start) == -1 || // No gap from previous
gapFilled) // Gap has already been dealt with
{
Contract <T> contract = contracts[inputContractIndex];
start = contract.Start;
end = contract.End;
inputContractIndex++;
gapFilled = false;
}
else
{
start = contracts[inputContractIndex - 1].End.Next();
end = contracts[inputContractIndex].Start.Previous();
gapFilled = true;
}
foreach (var timePeriod in start.EnumerateTo(end))
{
outputCurvePrices[outputContractIndex] = EvaluateSpline(timePeriod);
outputCurvePeriods[outputContractIndex] = timePeriod;
outputContractIndex++;
}
}
return(new DoubleCurve <T>(outputCurvePeriods, outputCurvePrices, weighting));
}
public static double[,] TrendAnalysis(double[,] z, int pow)
{
int row = z.GetLength(0);
int col = z.GetLength(1);
var n = (pow + 1) * (pow + 2) / 2;
//var m = row * col;
var result = new double[row, col];
var zv = new List <double>();
var corev = new List <double[]>();
int currXPow, currYPow, powIndex;
for (int i = 0; i < row; i++)
{
for (int j = 0; j < col; j++)
{
if (z[i, j] != 1.70141e38)
{
zv.Add(z[i, j]);
var co = new double[n];
powIndex = currXPow = currYPow = 0;
for (int k = 0; k < n; k++)
{
co[k] = Math.Pow(i + 1, currXPow) * Math.Pow(j + 1, currYPow);
if (currYPow == powIndex)
{
powIndex++;
currXPow = powIndex;
currYPow = 0;
}
else
{
currXPow--;
currYPow++;
}
}
corev.Add(co);
}
}
}
int count = corev.Count;
var coreArray = new double[count, n];
for (int i = 0; i < count; i++)
{
var co = corev[i];
for (int j = 0; j < n; j++)
{
coreArray[i, j] = co[j];
}
}
var zx = Vector.Dense(zv.ToArray());
var core = Matrix.DenseOfArray(coreArray);
var coreT = core.Transpose();
var temp = coreT * core;
var a = temp.Inverse() * coreT * zx;
var rv = (Matrix.DenseOfArray(GetCoreMatrix(row * col, n, col)) * a).AsArray();
int index = 0;
for (int i = 0; i < row; i++)
{
for (int j = 0; j < col; j++)
{
result[i, j] = rv[index];
index++;
}
}
return(result);
}
public override void RunAnalysis()
{
int len = Barray.Length;
// table phid-fz
Vector <double> Fz_vector = mybuilder.Dense(len, i => Harray[i] * lz);
Vector <double> phid_Fz_vector = mybuilder.Dense(len, i => Barray[i] * Sz + Fz_vector[i] * Pslot);
// table phid-fy
Vector <double> Fy_vector = mybuilder.Dense(len, i => Harray[i] * ly);
Vector <double> phid_Fy_vector = mybuilder.Dense(len, i => Barray[i] * Sy);
// reshape phid-fy, using interpolation to create new vector corresponding with phid_fy = phid_fz
Vector <double> phid_vector = mybuilder.DenseOfVector(phid_Fz_vector);//same phid table for all
LinearSpline ls = LinearSpline.Interpolate(phid_Fy_vector.ToArray(), Fy_vector.ToArray());
Vector <double> eqvFy_vector = mybuilder.Dense(len, i => ls.Interpolate(phid_vector[i]));
// table f_phid
Vector <double> f_phid = mybuilder.Dense(len, i => phid_vector[i] / Pd + Fz_vector[i] + eqvFy_vector[i] - (phiR - phiHFe - phid_vector[i]) / (PM + PFe + Pb));
var Results = new PMAnalyticalResults();
Results.Analyser = this;
this.Results = Results;
// calc phiD
ls = LinearSpline.Interpolate(f_phid.ToArray(), phid_vector.ToArray());
double phiD = ls.Interpolate(0);
double FM = (phiR - phiHFe - phiD) / (PM + PFe + Pb);
double phib = FM * Pb;
double phiFe = phiHFe + FM * PFe;
double phiM = phiD + phib + phiFe;
ls = LinearSpline.Interpolate(phid_vector.ToArray(), Fz_vector.ToArray());
double Fz = ls.Interpolate(phiD);
ls = LinearSpline.Interpolate(phid_vector.ToArray(), eqvFy_vector.ToArray());
double Fy = ls.Interpolate(phiD);
double Bdelta = phiD / (L * wd * 1e-6);
double BM = phiM / (L * wm * 1e-6);
double HM = FM / (lm * 1e-3);
double pc = phiM / (PM * FM);
double Bz = phiD / Sz;
double By = phiD / Sy;
double Vz = Sz * lz;
double Vy = Sy * ly;
double ro_ = 7800;//kg/m^3 - specific weight
double kPMz = 1.3 * Bz * Bz * ro_ * Vz;
double kPMy = 1.3 * By * By * ro_ * Vy;
Results.kPMz = kPMz;
Results.kPMy = kPMy;
// assign results
Results.phiD = phiD;
Results.Bdelta = Bdelta;
Results.phiM = phiM;
Results.BM = BM;
Results.FM = FM;
Results.HM = HM;
Results.pc = pc;
Results.phib = phib;
Results.phiFe = phiFe;
Results.Fz = Fz;
Results.Fy = Fy;
Results.Bz = Bz;
Results.By = By;
Results.wd = wd;
Results.gammaM = gammaM;
int q = Q / 3 / p / 2;
double kp = Math.Sin(Math.PI / (2 * 3)) / (q * Math.Sin(Math.PI / (2 * 3 * q)));
//Results.psiM = phiD * Nstrand * p * q * kp /*4 / Math.PI * Math.Sin(gammaM / 2 * Math.PI / 180)*/;
// Ld, Lq
//double In = 3;//Ampe whatever
//double Fmm = q * kp * Nstrand * In;
//double phidelta = (Fmm) / (1 / PMd + Fz / phiD + Fy / phiD) / 2;//2time,PMd=PM+Pdelta
//double psi = phidelta * q * kp * Nstrand;
//double LL = p * psi / In;
//Results.LL = LL;
//Results.Ld = 1.5 * LL;//M=-1/2L
//phidelta = (Fmm) / (1 / PMq + Fz / phiD + Fy / phiD) / 2;//
//psi = phidelta * q * kp * Nstrand;
//LL = p * psi / In;
//Results.Lq = 1.5 * LL;
// Resistant
Stator3Phase stator = Motor.Stator as Stator3Phase;
Results.R = stator.resistancePhase;
double delta2 = delta * 1.1;
double ns = 4 / Math.PI * kp * stator.NStrands * q;
double dmin = delta2;
double alphaM = Motor.Rotor is VPMRotor ? (Motor.Rotor as VPMRotor).alphaM : 180;
//double dmax = delta2 + lm / Math.Sin(alphaM);
double dmax = delta2 + Constants.mu_0 * L * wd * 1e-3 * (1 / (PM + PFe + Pb) + Fy / phiD + Fz / phiD);//* wd / wm2 * ((VPMRotor)Motor.Rotor).mu_M;
//double dmax = L * wd * 1e-3 * Constants.mu_0 / PMd;
//double dmin2 = 1 / Math.PI * ((180 - gammaM) * dmin + gammaM * dmax) * Math.PI / 180 - 2 / Math.PI * Math.Sin(gammaM * Math.PI / 180) * (dmax - dmin);
//double dmax2 = 1 / Math.PI * ((180 - gammaM) * dmin + gammaM * dmax) * Math.PI / 180 + 2 / Math.PI * Math.Sin(gammaM * Math.PI / 180) * (dmax - dmin);
//double a1 = 0.5 * (1 / dmin2 + 1 / dmax2) * 1e3;
//double a2 = 0.5 * (1 / dmin2 - 1 / dmax2) * 1e3;
//double a1 = 0.5 * (dmin + dmax) / (dmin * dmax) * 1e3;
//double a2 = 0.5 * (dmax - dmin) / (dmin * dmax) * 1e3;
// test override
double gm = gammaM * Math.PI / 180;
double a1 = 1 / Math.PI * (gm / dmax + (Math.PI - gm) / dmin) * 1e3;
double a2 = -2 / Math.PI * Math.Sin(gm) * (1 / dmax - 1 / dmin) * 1e3;
double L1 = (ns / 2) * (ns / 2) * Math.PI * Motor.Rotor.RGap * Motor.GeneralParams.MotorLength * 1e-6 * a1 * 4 * Math.PI * 1e-7;
double L2 = 0.5 * (ns / 2) * (ns / 2) * Math.PI * Motor.Rotor.RGap * Motor.GeneralParams.MotorLength * 1e-6 * a2 * 4 * Math.PI * 1e-7;
Results.dmin = dmin;
Results.dmax = dmax;
Results.L1 = L1;
Results.L2 = L2;
Results.Ld = 1.5 * (L1 - L2);
Results.Lq = 1.5 * (L1 + L2);
double psim = 2 * Math.Sin(gammaM * Math.PI / 180 / 2) * ns * Bdelta * Motor.Rotor.RGap * Motor.GeneralParams.MotorLength * 1e-6;
Results.psiM = psim;
// current demagnetizing
//Results.Ikm = 2 * phiR / PM / (kp * Nstrand * q); //old
double hck = Motor.Rotor is VPMRotor ? (Motor.Rotor as VPMRotor).Hck : 0;
Results.Ikm = Math.PI * (hck - HM) * lm * 1e-3 / (2 * q * Nstrand * kp);
dataValid = (Results.Ld > 0 && Results.Lq > 0 && Results.psiM >= 0 && gm > 0);
}
/// <summary>
/// Calculates a, b, c, d, e, and f using singular value decomposition.
/// lon = a*x + b*y + e;
/// lat = c*x + d*y + f;
/// </summary>
protected void createTransform()
{
Transform = null;
if (DataList.Count < 3)
{
Utils.errMsg("Need at least three data points for calibration.");
return;
}
// Define the matrices
MatrixBuilder <double> M = Matrix <double> .Build;
VectorBuilder <double> V = Vector <double> .Build;
int nPoints2 = 2 * DataList.Count;
Matrix <double> aa = M.Dense(nPoints2, 6);
Vector <double> bb = V.Dense(nPoints2);
MapData data = null;
int row;
for (int i = 0; i < DataList.Count; i++)
{
data = DataList[i];
row = 2 * i;
aa[row, 0] = data.X;
aa[row, 1] = data.Y;
aa[row, 4] = 1;
bb[row] = data.Lon;
row++;
aa[row, 2] = data.X;
aa[row, 3] = data.Y;
aa[row, 5] = 1;
bb[row] = data.Lat;
}
// Get the singular values
try {
Svd <double> svd = aa.Svd(true);
Matrix <double> u = svd.U;
Matrix <double> vt = svd.VT;
Matrix <double> w = svd.W;
Matrix <double> wi = w.Clone();
for (int i = 0; i < 6; i++)
{
if (wi[i, i] != 0)
{
wi[i, i] = 1.0 / wi[i, i];
}
}
Matrix <double> aainv = vt.Transpose() * wi.Transpose() * u.Transpose();
Vector <double> xx = aainv * bb;
double a = xx[0];
double b = xx[1];
double c = xx[2];
double d = xx[3];
double e = xx[4];
double f = xx[5];
Transform = new MapTransform(a, b, c, d, e, f);
} catch (Exception ex) {
Utils.excMsg("Failed to create calibration transform", ex);
Transform = null;
}
}
public void calc()
{
int len = Barray.Length;
// table phid-fz
Vector <double> Fz_vector = mybuilder.Dense(len, i => Harray[i] * lz);
Vector <double> phid_Fz_vector = mybuilder.Dense(len, i => Barray[i] * Sz + Fz_vector[i] * Pslot);
// table phid-fy
Vector <double> Fy_vector = mybuilder.Dense(len, i => Harray[i] * ly);
Vector <double> phid_Fy_vector = mybuilder.Dense(len, i => Barray[i] * Sy);
// reshape phid-fy, using interpolation to create new vector corresponding with phid_fy = phid_fz
Vector <double> phid_vector = mybuilder.DenseOfVector(phid_Fz_vector);//same phid table for all
LinearSpline ls = LinearSpline.Interpolate(phid_Fy_vector.ToArray(), Fy_vector.ToArray());
Vector <double> eqvFy_vector = mybuilder.Dense(len, i => ls.Interpolate(phid_vector[i]));
// table f_phid
Vector <double> f_phid = mybuilder.Dense(len, i => phid_vector[i] / Pd + Fz_vector[i] + eqvFy_vector[i] - (phiR - phiHFe - phid_vector[i]) / (PM + PFe + Pb));
// calc phiD
ls = LinearSpline.Interpolate(f_phid.ToArray(), phid_vector.ToArray());
phiD = ls.Interpolate(0);
FM = (phiR - phiHFe - phiD) / (PM + PFe + Pb);
phib = FM * Pb;
phiFe = phiHFe + FM * PFe;
phiM = phiD + phib + phiFe;
ls = LinearSpline.Interpolate(phid_vector.ToArray(), Fz_vector.ToArray());
Fz = ls.Interpolate(phiD);
ls = LinearSpline.Interpolate(phid_vector.ToArray(), eqvFy_vector.ToArray());
Fy = ls.Interpolate(phiD);
}
private static void Assignment1(int correctionLoop, double T = 0.01, int tmax = 10, bool verbose = true,
string assignmentPath = "C:/git/Computer-Aided-Analysis-and-Design/Homework_5/Files/Assignment1/", bool saveToFile = false)
{
Console.WriteLine("ASSIGNMENT 1");
var start = VectorBuilder.Dense(new[] { 1.0, 1.0 });
var A = MatrixBuilder.Dense(2, 2, new[] { 0.0, -1.0, 1.0, 0.0 });
var B = MatrixBuilder.Dense(2, 2);
Func <double, double> rt = _ => 0.0;
var euler = Euler(assignmentPath + "euler.txt", tmax, T, start, A, B, rt, verbose, saveToFile);
var reverseEuler = ReverseEuler(assignmentPath + "reverseEuler.txt", tmax, T, start, A, B, rt, verbose, saveToFile);
var trapezoid = Trapezoid(assignmentPath + "trapezoid.txt", tmax, T, start, A, B, rt, verbose, saveToFile);
var rungeKutta = RungeKutta(assignmentPath + "rungeKutta.txt", tmax, T, start, A, B, rt, verbose, saveToFile);
// var pece = Pece(assignmentPath + "pece.txt", tmax, T, start, A, B, rt, verbose, correctionLoop, saveToFile);
// var pecece = Pecece(assignmentPath + "pecece.txt", tmax, T, start, A, B, rt, verbose, correctionLoop, saveToFile);
}
public double Normalize(int joint_index, double theta)
{
double u = Robot.Joint_upper_limit[joint_index];
double l = Robot.Joint_lower_limit[joint_index];
if (check_limits)
{
if (!(l < theta && theta < u))
{
Vector <double> a = v_builder.DenseOfArray(new[] { -1.0, 1.0 });
a = a * 2 * Math.PI;
Vector <double> b = a.Add(theta);
int index = -1;
for (int i = 0; i < b.Count; i++)
{
if (l < b[i] && b[i] < u)
{
index = i;
break;
}
}
if (index == -1)
{
return(default(double));
}
else
{
theta = theta + a[index];
}
}
}
if (use_last_joints)
{
double diff = Last_joints[joint_index] - theta;
int n_diff = (int)Math.Floor(diff / (Math.PI * 2.0));
double r_diff = diff % (Math.PI * 2.0);
if (r_diff > Math.PI)
{
n_diff += 1;
}
if (Math.Abs(n_diff) > 0)
{
if (!check_limits)
{
theta += 2 * Math.PI * n_diff;
}
else
{
double theta_vs = theta + 2 * Math.PI;
Vector <double> theta_v = v_builder.Dense(Math.Abs(n_diff));
for (int i = 0; i < theta_v.Count; i++)
{
if (n_diff > 0)
{
theta_v[i] = n_diff - i;
}
else if (n_diff < 0)
{
theta_v[i] = n_diff + i;
}
}
theta_v = theta_v * 2 * Math.PI;
theta_v = theta_v.Add(theta);
int index = -1;
for (int i = 0; i < theta_v.Count; i++)
{
if (l < theta_v[i] && theta_v[i] < u)
{
index = i;
break;
}
}
if (index != -1)
{
theta = theta_v[index];
}
}
}
}
return(theta);
}
public Plane(Ship ship)
{
PlaneInertia = new()
{
Ixx = 74608.24,// 转动惯量 kg/m2
Iyy = 227616.4,
Izz = 231114.32,
Ixz = 0,
WingS = 62.006, // 翼面积
WingC = 4.218, // 平均气动弦长
WingL = 14.70, // 翼展
Mass = 16382.85, // 空重 kg
TMax = 122.6 * 1000, // 军用推力（两台）
Rou = 1.225, // 空气密度
G = 9.8
};
DesiredParameter = new()
{
Alpha = 10 * Pi / 180, // 期望迎角 9.1
Chi = 0 * Pi / 180, // 期望航向角
Gamma = -3.5 * Pi / 180, // 期望爬升角
Vk = 62, // 期望速度 71
EngineDelta = 0 * Pi / 180 // 发动机安装角
};
Flow = 0.5 * PlaneInertia.Rou * Math.Pow(Vk, 2);
T = PlaneInertia.TMax * DeltaP;
ThrustRange = PlaneInertia.TMax * DeltaPRange;
ThrustRateRange = PlaneInertia.TMax * DeltaPRateRange;
CalculatePneumaticParameters();
CalculateForceAndMoment();
Initialize(ship);
Theta = Gamma + Alpha;
DeltaTEFDesired = DeltaTEF;
DesiredPosition = vb.Dense(3, Position[0]);
DesiredPosition.SetSubVector(
1, 2, HelperFunction.ideal_path(Position, ship.Position, ship.Theta, ship.Psi));
}
void AddListeners()
{
}
void Initialize(Ship ship)
{
Vector <double> p_d_2p;
if (l_path_0 > 1620)
{
double x_d_2p = -1620 * Cos(ship.Theta) * Cos(ship.Gamma) - (l_path_0 - 1620); // 期望点坐标 P系下表示
double y_d_2p = 1620 * Sin(ship.Theta) * Cos(ship.Gamma);
double z_d_2p = 1620 * Sin(ship.Gamma);
p_d_2p = vb.Dense(new double[] { x_d_2p, y_d_2p, z_d_2p }); // 期望点坐标 P系下表示
}
else
{
double x_d_2p = -(l_path_0 - l_path) * Cos(ship.Theta) * Cos(ship.Gamma); // 期望点坐标 P系下表示
double y_d_2p = (l_path_0 - l_path) * Sin(ship.Theta) * Cos(ship.Gamma);
double z_d_2p = (l_path_0 - l_path) * Sin(ship.Gamma);
p_d_2p = vb.Dense(new double[] { x_d_2p, y_d_2p, z_d_2p }); // 期望点坐标 P系下表示
}
Matrix <double> R_i2p = mb.DenseOfArray(new double[, ] {
{ Cos(ship.Psi), Sin(ship.Psi), 0 }, { -Sin(ship.Psi), Cos(ship.Psi), 0 }, { 0, 0, 1 }
});
Matrix <double> R_p2i = R_i2p.Transpose();
Vector <double> p_d_2i = R_p2i * p_d_2p + ship.Position; // 期望点坐标 I系下表示
double kai_f = 0;
double gamma_f = 0;
Matrix <double> R_i2f = mb.DenseOfArray(new double[, ] {
{ Cos(gamma_f) * Cos(kai_f), Cos(gamma_f) * Sin(kai_f), -Sin(gamma_f) },
{ -Sin(kai_f), Cos(kai_f), 0 },
{ Sin(gamma_f) * Cos(kai_f), Sin(gamma_f) * Sin(kai_f), Cos(gamma_f) }
});
Matrix <double> R_f2i = R_i2f.Transpose();
Vector <double> p_b_2f = vb.Dense(new double[] { 0, 5, 10 }); // 飞机在F系中初始位置
x_b_2f = p_b_2f[0];
y_b_2f = p_b_2f[1];
z_b_2f = p_b_2f[2];
Position = R_f2i * p_b_2f + p_d_2i; // 飞机在I系中初始位置
Gamma = 0; // 初始爬升角
Chi = -ship.Theta; // 初始航向角
Vector <double> omega_d_2f = R_i2f * ship.omega_d_2i;
double omega_dx_2f = omega_d_2f[0];
double omega_dy_2f = omega_d_2f[1];
double omega_dz_2f = omega_d_2f[2];
Vector <double> omega_f_2f = vb.Dense(new double[] { omega_dx_2f, omega_dy_2f, omega_dz_2f });
omega_fx_2f = omega_f_2f[0];
omega_fy_2f = omega_f_2f[1];
omega_fz_2f = omega_f_2f[2];
kai_b2f = Chi - kai_f;
gamma_b2f = Gamma - gamma_f;
}
public void Record()
{
RecordPlaneStateEvent?.Invoke(this, null);
}
void CalculatePneumaticParameters()
{
double WingC = PlaneInertia.WingC;
double WingS = PlaneInertia.WingS;
double WingL = PlaneInertia.WingL;
double CM_delta_tef = 0.001 * 180 / Pi;
double CM_delta_lef = 0;
double Cnormal_delta_tef = 0.009 * 180 / Pi;
double Cnormal_delta_lef = Cnormal_delta_tef * 0.5; // 推测数据，缺少实测数据
double Caxis_delta_tef = 0;
double Caxis_delta_lef = 0;
// 升力系数相关参数 弧度制
double CY_alpha3 = -20.779;
double CY_alpha2 = 1.1682;
double CY_alpha1 = 3.8091;
double CY_0 = 0.12;
CY_alpha = CY_alpha3 * Math.Pow(Alpha, 2) + CY_alpha2 * Alpha + CY_alpha1;
double CY_delta_e = 0.6;
double CY_delta_lef = 0;
CY_delta_tef = 0.02 * 57.3;
CY = CY_0
+ CY_alpha * Alpha
+ CY_delta_e * DeltaE
+ CY_delta_lef * DeltaLEF // leading - egde flap && tailing - edge flap
+ CY_delta_tef * DeltaTEF; // lift coefficient
// 阻力系数相关参数 弧度制
double CD_alpha2 = 0.3009;
double CD_alpha1 = -0.0622;
double CD_alpha0 = -1.4994;
double CD_0 = -0.0019 + 0.0109;
CD_alpha = CD_alpha2 * Alpha + CD_alpha1;
double CD_delta_e = 0.04;
double CD_delta_lef = 0;
double CD_delta_tef = 0.002 * 57.3;
double CD_Ma;
if (Vk / 340 <= 0.81)
{
CD_Ma = 0;
}
else
{
CD_Ma = 0.1007 * Math.Pow(Vk / 340, 3) - 0.4653 * Math.Pow(Vk / 340, 2) + 0.6903 * (Vk / 340) - 0.3074;
}
double CD_beta1 = 0.4946;
double CD_beta = CD_beta1 * Beta;
CD = CD_0
+ CD_beta * Beta // 侧滑阻力
+ CD_alpha * Alpha // 迎角引起的阻力
+ CD_delta_e * Math.Abs(DeltaE) // 升降舵偏转引起的阻力。
+ CD_delta_lef * DeltaLEF // leading - egde flap引起的阻力认为是0 && tailing - edge flap
+ CD_delta_tef * DeltaTEF // 襟翼引起的阻力
+ 0.1 * Math.Pow(CY, 2) // 升致阻力
+ CD_Ma; // 马赫导致的阻力 // drag coefficient
// 侧力系数 弧度制
CC_beta = -1;
CC = CC_beta * Beta;
// 滚转力矩相关参数 弧度制
CL_beta = -0.1;
CL_delta_a = 0.12;
CL_delta_r = 0.01;
double CL_p = -0.4;
double CL_r = 0.15;
CL = CL_beta * Beta // beta引起的滚转力矩
+ CL_delta_a * DeltaA
+ CL_delta_r * DeltaR
+ WingL / 2 / Vk * CL_p * P
+ WingL / 2 / Vk * CL_r * R; // rolling moment coefficient
// 俯仰力矩相关参数 弧度制
CM_delta_e = 0.35 * (Vk / 340) - 1.1;
double CM_q = -23;
CM_alpha = -0.1;
double CM_alpha_dot = -9;
CM = CM_alpha * Alpha // 由alpha引起的俯仰力矩
+ CM_delta_e * DeltaE // 升降舵产生的俯仰力矩
+ WingC / 2 / Vk * CM_q * Q; // 俯仰角速度产生的俯仰力矩
// 偏航力矩相关参数 弧度制
CN_beta = 0.12;
CN_delta_r = -0.15;
double CN_r = -0.15;
CN_delta_a = 0;
CN = CN_beta * Beta
+ CN_delta_r * DeltaR
+ WingL / 2 / Vk * CN_r * R; // yawing moment coefficient
}
void CalculateForceAndMoment()
{
Y = Flow * PlaneInertia.WingS * CY;
D = Flow * PlaneInertia.WingS * CD;
C = Flow * PlaneInertia.WingS * CC;
L = Flow * PlaneInertia.WingS * PlaneInertia.WingL * CL;
M = Flow * PlaneInertia.WingS * PlaneInertia.WingC * CM;
N = Flow * PlaneInertia.WingS * PlaneInertia.WingL * CN;
}
