//Calculates the next state vector normalized with respect to time.
Vector <double> CalculateNextStates(LinearStateSpaceModel autonomousSS)
{
Vector <double> currentStates = CurrentStateVector();
Vector <double> changeInStates = autonomousSS.A.Multiply(currentStates);
//print("Change in states:\n" + changeInStates.ToString());
return(currentStates.Add(changeInStates * Time.deltaTime));
}
void PrettyPrintStateSpaceModel(LinearStateSpaceModel ss = null)
{
if (ss == null)
{
print("The PrettyPrintStateSpaceModel() method was called, but the state space model wasn't defined.");
}
else
{
print("-------------------\nLinear State Space Model:");
print("A:\n" + ss.A.ToString());
print("B:\n" + ss.B.ToString());
print("C:\n" + ss.C.ToString());
print("D:\n" + ss.D.ToString());
print("Current state:\n" + CurrentStateVector().ToString());
print("-------------------");
}
}
void CreateAutonomousStateSpaceModel()
{
double[,] AMatrix = { { 0, 1, 0, 0, 0, 0 },
{ -0.0032, -0.3278, 0, 0, 639.7634, -35.8847 },
{ 0, 0, 0, 1, 0, 0 },
{ 0, 0, -1000, -110, 0, 0 },
{ 0, 0, 0, 0, 0, 1 },
{ -0.01, -1.0111, 0, 0, -1111, -110.6822 } };
double[,] BMatrix = { { 0, 0 },
{ 0, 0 },
{ 0, 0 },
{ 0, 0 },
{ 0, 0 },
{ 0, 0 } };
double[,] CMatrix = { { 1, 0, 0, 0, 0, 0 },
{ 0, 1, 0, 0, 0, 0 },
{ 0, 0, 1, 0, 0, 0 },
{ 0, 0, 0, 1, 0, 0 },
{ 0, 0, 0, 0, 1, 0 },
{ 0, 0, 0, 0, 0, 1 } };
autonomousSS = new LinearStateSpaceModel(AMatrix, BMatrix, CMatrix);
}
void CreateLinearizedStateSpaceModel()
{
double[,] AMatrix = { { 0, 1, 0, 0, 0, 0 },
{ 0, 0, 0, 0, engines.mainThrustMultiplier / _rb.mass, 0 },
{ 0, 0, 0, 1, 0, 0 },
{ 0, 0, 0, 0, -engines.stabilizerThrustMultiplier / _rb.mass, 0 },
{ 0, 0, 0, 0, 0, 1 },
{ 0, 0, 0, 0, 0, 0 } };
double[,] BMatrix = { { 0, 0 },
{ 0, 1 / _rb.mass },
{ 0, 0 },
{ 1 / _rb.mass, 0 },
{ 0, 0 },
{ 0, 1 / _rb.inertia } };
double[,] CMatrix = { { 1, 0, 0, 0, 0, 0 },
{ 0, 1, 0, 0, 0, 0 },
{ 0, 0, 1, 0, 0, 0 },
{ 0, 0, 0, 1, 0, 0 },
{ 0, 0, 0, 0, 1, 0 },
{ 0, 0, 0, 0, 0, 1 } };
stateSpaceModel = new LinearStateSpaceModel(AMatrix, BMatrix, CMatrix);
}
void CreateLQRStateSpaceModel()
{
//multiply A by 10 to get original
double[,] AMatrix = { { 0, .1, 0, 0, 0, 0 },
{ -.32, -3.18, 0, 0, -50.14, -3.53 },
{ 0, 0, 0, .1, 0, 0 },
{ 0, -.01, -.32, -.4, -2.95, 0 },
{ 0, 0, 0, 0, 0, .1 },
{ -.98, -9.8, 0, 0, -463.08, -10.89 } };
double[,] BMatrix = { { 0, 0 },
{ 0, 0 },
{ 0, 0 },
{ 0, 0 },
{ 0, 0 },
{ 0, 0 } };
double[,] CMatrix = { { 1, 0, 0, 0, 0, 0 },
{ 0, 1, 0, 0, 0, 0 },
{ 0, 0, 1, 0, 0, 0 },
{ 0, 0, 0, 1, 0, 0 },
{ 0, 0, 0, 0, 1, 0 },
{ 0, 0, 0, 0, 0, 1 } };
LQRSS = new LinearStateSpaceModel(AMatrix, BMatrix, CMatrix);
}
