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