public static (Vector <double>, Matrix <double>) Step(Func <int, Vector <double>, Vector <double> > Phi,
Func <int, Vector <double>, Vector <double> > Psi,
Matrix <double> Rw,
Matrix <double> Rnu,
UTParams p1,
UTParams p2,
int t,
Vector <double> y,
Vector <double> xHat_,
Matrix <double> P_)
{
UnscentedTransform.Transform(x => Phi(t, x), xHat_, P_, Rw, p1, out Vector <double> Xtilde, out _, out Matrix <double> Ptilde);
UnscentedTransform.Transform(x => Psi(t, x), Xtilde, Ptilde, Rnu, p2, out Vector <double> Ytilde, out Matrix <double> PXY, out Matrix <double> PYtilde);
Matrix <double> K = PXY * PYtilde.Inverse();
return(Xtilde + K * (y - Ytilde), Ptilde - K * PYtilde * K.Transpose());
}
/// <summary>
/// Performs a step of Unscented Kalman Filter given the particular unscented transform parameters
/// for forecast and correction phases
/// </summary>
/// <param name="Phi1">State transformation: a nonlinear function which determines the dynamics: x_{t+1} = Phi_1(x_t) + Phi_2(x_t) W_t</param>
/// <param name="Phi2">Noise multiplicator in the dynamics equation: x_{t+1} = Phi(x_t) + W_t</param>
/// <param name="Psi1">Observations transformation: a nonlinear function which determines the relation between the state and the observations: y_t = Psi_1(x_t) + Psi_2(x_t) Nu_t</param>
/// <param name="Psi2">Noise multiplicator in the observations equation: y_t = Psi_1(x_t) + Psi_2(x_t) Nu_t</param>
/// <param name="Mw">Mean of the noise in the dynamics equation </param>
/// <param name="Rw">Covariance matrix of the state disturbances</param>
/// <param name="Mnu">Mean of the noise in the obseration equation </param>
/// <param name="Rnu">Convariance matrix of the observation noise</param>
/// <param name="p1">Unscented transfrom parameters for the forecast phase</param>
/// <param name="p2">Unscented transfrom parameters for the correction phase</param>
/// <param name="t">Current step time instant</param>
/// <param name="y">Observations on the current step</param>
/// <param name="xHat_">Estimate on the previous step</param>
/// <param name="P_">Approximated previous step error covariance</param>
/// <param name="xHat">Returns: current step estimate</param>
/// <param name="P">Returns: approximated current step error covariance</param>
public static (Vector <double>, Matrix <double>) Step(Func <int, Vector <double>, Vector <double> > Phi1,
Func <int, Vector <double>, Matrix <double> > Phi2,
Func <int, Vector <double>, Vector <double> > Psi1,
Func <int, Vector <double>, Matrix <double> > Psi2,
Vector <double> Mw,
Matrix <double> Rw,
Vector <double> Mnu,
Matrix <double> Rnu,
UTParams p1,
UTParams p2,
int t,
Vector <double> y,
Vector <double> xHat_,
Matrix <double> P_)
{
try
{
UnscentedTransform.Transform(x => Phi1(t, x) + Phi2(t, x) * Mw, xHat_, P_, Phi2(t, xHat_) * Rw * Phi2(t, xHat_).Transpose(), p1, out Vector <double> Xtilde, out _, out Matrix <double> Ptilde);
//UnscentedTransform.Transform(x => Phi1(t, x), xHat_, P_, Rw, p1, out Vector<double> Xtilde2, out _, out Matrix<double> Ptilde2);
UnscentedTransform.Transform(x => Psi1(t, x) + Psi2(t, x) * Mnu, Xtilde, Ptilde, Psi2(t, Xtilde) * Rnu * Psi2(t, Xtilde).Transpose(), p2, out Vector <double> Ytilde, out Matrix <double> PXY, out Matrix <double> PYtilde);
//UnscentedTransform.Transform(x => Psi1(t, x), Xtilde, Ptilde, Rnu, p2, out Vector<double> Ytilde2, out Matrix<double> PXY2, out Matrix<double> PYtilde2);
Matrix <double> K = PXY * PYtilde.Inverse();
return(Xtilde + K * (y - Ytilde), Ptilde - K * PYtilde * K.Transpose());
}
catch (Exception e)
{
Console.WriteLine(e.Message);
return(xHat_, P_);
}
//Matrix<double> K2 = PXY2 * PYtilde2.Inverse();
//Vector<double> xHat2 = Xtilde2 + K2 * (y - Ytilde2);
//Matrix<double> PHat2 = Ptilde2 - K2 * PYtilde2 * K2.Transpose();
//Console.WriteLine(xHat - xHat2);
//Console.WriteLine(PHat - PHat2);
}
