void IAnimoji.SetLocal(Vector3 position, Quaternion rotation)
{
// Appliy Kalman Filter
float[] z = { position.x, position.y, position.z, rotation.x, rotation.y, rotation.z, rotation.w };
float[] nz = filter.Update(z, Time.deltaTime);
// kalman-filtered position
Vector3 np = new Vector3(nz[0], nz[1], nz[2]);
// kalman-filtered rotation
Quaternion nr = new Quaternion(nz[3], nz[4], nz[5], nz[6]);
this.transform.localPosition = (correction * np);
this.transform.localRotation = iR * nr;
}
void FixedUpdate()
{
/* Get current sample time */
float T = Time.deltaTime;
totalTime += T;
if (totalTime - timerSensors >= SAMPLE_TIME_SENSORS)
{
/* Read sensors and update state estimator */
ac.GetSensorMeasurements();
/* Filter barometer measurements */
altFilter.Update(ac.baroAltitude);
timerSensors = totalTime;
}
if (totalTime - timerStateEstimation >= SAMPLE_TIME_KALMAN)
{
float T_KF = totalTime - timerStateEstimation;
/* Update state estimate */
kf.Update(ac.gyroQ, ac.accX, ac.accZ, ac.pitotVa, T_KF);
timerStateEstimation = totalTime;
}
/* Get controller outputs */
if (totalTime - timerCtrlAirspeed >= SAMPLE_TIME_CTRL_AIRSPEED)
{
float T_CTRL_AIRSPEED = totalTime - timerCtrlAirspeed;
throttleSetting = piAirspeed.Update(airspeedSetpoint, kf.VaFilt, T_CTRL_AIRSPEED);
timerCtrlAirspeed = totalTime;
}
if (totalTime - timerCtrlAltitude >= SAMPLE_TIME_CTRL_ALTITUDE)
{
float T_CTRL_ALTITUDE = totalTime - timerCtrlAltitude;
pitchAngleSetpoint = piAltitude.Update(altitudeSetpoint, altFilter.output, T_CTRL_ALTITUDE);
timerCtrlAltitude = totalTime;
}
if (totalTime - timerCtrlPitch >= SAMPLE_TIME_CTRL_PITCH)
{
float T_CTRL_PITCH = totalTime - timerCtrlPitch;
piPitch.Update(pitchAngleSetpoint, kf.theta, T_CTRL_PITCH);
timerCtrlPitch = totalTime;
}
/* Calculate elevator deflection */
elevatorDeflection = -actElevator.Update(piPitch.output, T);
/* Calculate engine thrust */
engineThrust = engine.GetThrust(actEngine.Update(throttleSetting, T));
/* Update aircraft state vector */
ac.Update(engineThrust, elevatorDeflection, T);
/* Update 3D position and view */
trans.eulerAngles = new Vector3(-ac.acState[3] * RADTODEG, 0.0f, 0.0f);
trans.position = new Vector3(0.0f, ac.acState[5], ac.acState[4]);
camTrans.position = trans.position + camOffset;
/* Update GUI */
UpdateGUI();
/* Log data */
if (csvLogging)
{
csv.AddLine(new string[] { totalTime.ToString("F6"),
ac.acState[0].ToString("F6"), ac.acState[1].ToString("F6"), ac.acState[2].ToString("F6"), ac.acState[3].ToString("F6"), ac.acState[4].ToString("F6"), ac.acState[5].ToString("F6"),
ac.GetAirspeed(ac.acState[0], ac.acState[1]).ToString("F6"), ac.GetAngleOfAttack(ac.acState[0], ac.acState[1]).ToString("F6"),
kf.qFilt.ToString("F6"), kf.axFilt.ToString("F6"), kf.azFilt.ToString("F6"), kf.VaFilt.ToString("F6"), altFilter.output.ToString("F6"), kf.theta.ToString("F6"),
elevatorDeflection.ToString("F6"), throttleSetting.ToString("F6"),
airspeedSetpoint.ToString("F6"), pitchAngleSetpoint.ToString("F6"), altitudeSetpoint.ToString("F6") });
}
}