public void UpdateState(double dt, Disturbance disturbance) { // plane.current_v = [Sin(plane.current_miu), plane.current_alpha * Cos(plane.current_miu)]'; //dt = e.data{ 1}; //current_X3_dot = e.data{ 2}; //current_X3 = current_X3 + current_X3_dot * dt; }
public Simulation() { InitializeInitialParameters(); Ship = new Ship(); Plane = new Plane(Ship); Disturbance = new Disturbance(); PositionLoop = new PositionLoop(Plane, Ship); FlightPathLoop = new FlightPathLoop(Plane, Ship); AttitudeLoop = new AttitudeLoop(Plane, Ship); AngularRateLoop = new AngularRateLoop(Plane, Ship); Record = new SimulationRecord(); Plane.X1ChangedEvent += PositionLoop.OnUpdateState; Plane.X2ChangedEvent += FlightPathLoop.OnUpdateState; Plane.X3ChangedEvent += AttitudeLoop.OnUpdateState; Plane.X4ChangedEvent += AngularRateLoop.OnUpdateState; Plane.RecordPlaneStateEvent += Record.OnRecordPlaneState; DataSendTimer = new Timer(100); DataSendTimer.Elapsed += (sender, e) => { if (DataQueue.TryDequeue(out double data)) { connection.InvokeAsync("SendData", "user", data); } if (DataQueue.IsEmpty) { DataSendTimer.Stop(); Console.WriteLine("Timer Stoped"); } }; DataQueue = new ConcurrentQueue <double>(); Console.WriteLine("text"); }
void Reset() { step_count = 0; current_time = 0; Ship.reset(); Plane.Reset(Ship); PositionLoop.Reset(); FlightPathLoop.Reset(); AttitudeLoop.Reset(); AngularRateLoop.Reset(); Disturbance.reset(); }
void SingleStep() { step_count++; current_time = dt * step_count; PositionLoop.CalculateObservation(); FlightPathLoop.CalculateObservation(); AttitudeLoop.CalculateObservation(); AngularRateLoop.CalculateObservation(); FlightPathLoop.CalculateNonlinearObserver(dt, Disturbance); AngularRateLoop.CalculateNonlinearObserver(dt, Disturbance); PositionLoop.CalculatePrescribedParameter(); Ship.CalculateCompensation(dt, Plane, PositionLoop, step_count); PositionLoop.CalculateState(dt, null); PositionLoop.CalculateOutput(dt, current_time, step_count); PositionLoop.CalculateLimiter(dt); FlightPathLoop.CalculateState(dt, PositionLoop.U1); FlightPathLoop.CalculateOutput(); FlightPathLoop.CalculateLimiter(dt); FlightPathLoop.CalculateFilter(dt); AttitudeLoop.CalculateState(dt, FlightPathLoop.U2, FlightPathLoop.DeriveX2[1]); AttitudeLoop.CalculateOutput(); AttitudeLoop.CalculateLimiter(dt); AngularRateLoop.CalculateState(dt, AttitudeLoop.U3); AngularRateLoop.CalculateOutput(dt, current_time, step_count); AngularRateLoop.CalculateLimiter(dt); AngularRateLoop.CalculateFilter(dt); Plane.UpdateState(dt, Disturbance); Ship.UpdateState(dt); Disturbance.updateWind(Plane, Ship, step_count); Plane.Record(); Ship.record(); PositionLoop.Record(dt); FlightPathLoop.Record(dt); AttitudeLoop.Record(dt); AngularRateLoop.Record(dt); Record.time_record.Add(current_time); if (Plane.l_path > 1000) { landing_gear = true; tail_hook = true; } }
public void CalculateNonlinearObserver(double dt, Disturbance disturbance) { return; }
public void UpdateState(double dt, Disturbance disturbance) { CalculatePneumaticParameters(); CalculateForceAndMoment(); disturbance.updateState(this); double Ixx = PlaneInertia.Ixx; double Iyy = PlaneInertia.Iyy; double Izz = PlaneInertia.Izz; double Ixz = PlaneInertia.Ixz; double Mass = PlaneInertia.Mass; double TMax = PlaneInertia.TMax; double Rou = PlaneInertia.Rou; double G = PlaneInertia.G; double AlphaDesired = DesiredParameter.Alpha; // 期望迎角 9.1 double ChiDesired = DesiredParameter.Chi; // 期望航向角 double GammaDesired = DesiredParameter.Gamma; // 期望爬升角 double VkDesired = DesiredParameter.Vk; // 期望速度 71 double EngineDelta = DesiredParameter.EngineDelta; // 发动机安装角 double current_p_dot = 1 / (Ixx * Izz - Math.Pow(Ixz, 2)) * ((Iyy * Izz - Math.Pow(Izz, 2) - Math.Pow(Ixz, 2)) * Q * R + (Ixx * Ixz + Izz * Ixz - Iyy * Ixz) * P * Q + Izz * L + Ixz * N) + disturbance.disturbance_p; double current_q_dot = 1 / Iyy * ((Izz - Ixx) * P * R - Ixz * Math.Pow(P, 2) + Ixz * Math.Pow(R, 2) + M) + disturbance.disturbance_q; double current_r_dot = 1 / (Ixx * Izz - Math.Pow(Ixz, 2)) * ((Math.Pow(Ixx, 2) + Math.Pow(Ixz, 2) - Ixx * Iyy) * P * Q + (Iyy * Ixz - Ixx * Ixz - Izz * Ixz) * Q * R + Ixz * L + Ixx * N) + disturbance.disturbance_r; Vector <double> current_X4_dot = vb.Dense(new[] { current_p_dot, current_q_dot, current_r_dot }); P += current_p_dot * dt; Q += current_q_dot * dt; R += current_r_dot * dt; X4ChangedEvent?.Invoke(this, new XChangedEventArgs(dt, current_X4_dot)); // 欧拉角 double current_phi_dot = P + Tan(Theta) * (Q * Sin(Phi) + R * Cos(Phi)); double current_theta_dot = Q * Cos(Phi) - R * Sin(Phi); double current_psi_dot = 1 / Cos(Theta) * (Q * Sin(Phi) + R * Cos(Phi)); Phi += current_phi_dot * dt; Theta += current_theta_dot * dt; Psi += current_psi_dot * dt; // 绕质心运动学方程 double current_beta_dot = P * Sin(Alpha) - R * Cos(Alpha) - GammaDerive * Sin(Miu) + ChiDerive * Cos(Miu) * Cos(Gamma); double current_miu_dot = (P * Cos(Alpha) + R * Sin(Alpha) + GammaDerive * Sin(Beta) * Cos(Miu) + ChiDerive * (Sin(Gamma) * Cos(Beta) + Sin(Beta) * Sin(Miu) * Cos(Gamma))) / Cos(Beta); double current_alpha_dot = (Q * Cos(Beta) - (P * Cos(Alpha) + R * Sin(Alpha)) * Sin(Beta) - GammaDerive * Cos(Miu) - ChiDerive * Sin(Miu) * Cos(Gamma)) / Cos(Beta); Vector <double> current_X3_dot = vb.Dense(new[] { current_theta_dot, current_beta_dot, current_miu_dot }); // update state parameters Alpha += current_alpha_dot * dt; Beta += current_beta_dot * dt; Miu += current_miu_dot * dt; X3ChangedEvent?.Invoke(this, new XChangedEventArgs(dt, current_X3_dot)); // 质心动力方程 double current_kai_dot = 1 / (Mass * Vk * Cos(Gamma)) * (T * (Sin(Alpha + EngineDelta) * Sin(Miu) - Cos(Alpha + EngineDelta) * Sin(Beta) * Cos(Miu)) + C * Cos(Miu) + Y * Sin(Miu)) + disturbance.disturbance_kai; double current_gamma_dot = -1 / (Mass * Vk) * (T * (-Sin(Alpha + EngineDelta) * Cos(Miu) - Cos(Alpha + EngineDelta) * Sin(Beta) * Sin(Miu)) + C * Sin(Miu) - Y * Cos(Miu) + Mass * G * Cos(Gamma)) + disturbance.disturbance_gamma; Vector <double> current_X2_dot = vb.Dense(new[] { current_kai_dot, current_gamma_dot }); double current_Vk_dot = 1 / Mass * (T * Cos(Alpha + EngineDelta) * Cos(Beta) - D - Mass * G * Sin(Gamma)) + disturbance.disturbance_Vk; // update state parameters Chi += current_kai_dot * dt; Gamma += current_gamma_dot * dt; DeltaP = T / TMax; Vk += current_Vk_dot * dt; Flow = 0.5 * Rou * Math.Pow(Vk, 2); GammaDerive = current_gamma_dot; ChiDerive = current_kai_dot; X2ChangedEvent?.Invoke(this, new XChangedEventArgs(dt, current_X2_dot)); // 质心运动方程 double current_x_dot = Vk * Cos(Gamma) * Cos(Chi); double current_y_dot = Vk * Cos(Gamma) * Sin(Chi); double current_z_dot = -Vk *Sin(Gamma); Vector <double> current_X1_dot = vb.Dense(new[] { current_y_dot, current_z_dot }); // update state parameters Position[0] += current_x_dot * dt; Position[1] += current_y_dot * dt; Position[2] += current_z_dot * dt; X1ChangedEvent?.Invoke(this, new XChangedEventArgs(dt, current_X1_dot)); }
public void UpdateState(double dt, Disturbance disturbance) { //dt = e.data{ 1}; //current_X1_dot = e.data{ 2}; //current_X1 = current_X1 + current_X1_dot * dt; }
public void CalculateNonlinearObserver(double dt, Disturbance disturbance) { throw new NotImplementedException(); }