/// <summary>
/// Provides the unscented transform estimate for the array X = [x_0,...,x_N] of random variable x samples
/// given the array of observations Y = [y_0,...,y_N], where y_i = Phi(x_i) + Nu_i.
/// Parameters of the unscented transform utParams should be initialized.
/// </summary>
/// <param name="Phi">Transformation: a nonlinear function which determines the transformation of the random vector variable: y = Phi(x) + nu</param>
/// <param name="X">Array of initial variable x samples</param>
/// <param name="Y">Array of transformed variable y = Phi(x) + nu samples</param>
/// <param name="mX">Mean of x</param>
/// <param name="KX">Cov of x</param>
/// <param name="KNu">Cov of the noize nu</param>
/// <param name="mErr_UT">Returns: estimation error mean vector</param>
/// <param name="KErr_UT">Returns: estimation error covariance marix</param>
/// <param name="KErrTh_UT">Returns: estimation error theoretical covariance marix</param>
/// <returns>Array of estimates \hat{X} = [\hat{x}_0,...,\hat{x}_N]</returns>
public Vector <double>[] Estimate(Func <Vector <double>, Vector <double> > Phi, Vector <double>[] X, Vector <double>[] Y, Vector <double> mX, Matrix <double> KX, Matrix <double> KNu,
out Vector <double> mErr_UT, out Matrix <double> KErr_UT, out Matrix <double> KErrTh_UT)
{
UnscentedTransform.Transform(x => Phi(x), mX, KX, KNu, utParams, out Vector <double> M_UT, out Matrix <double> Kxy_UT, out Matrix <double> Kyy_UT);
Matrix <double> P_UT = Kxy_UT * ((Kyy_UT).PseudoInverse());
KErrTh_UT = KX - P_UT * Kyy_UT * P_UT.Transpose();
Vector <double>[] Xhat_UT = Y.Select(y => mX + P_UT * (y - M_UT)).ToArray();
Vector <double>[] Err_UT = Xhat_UT.Subtract(X);
mErr_UT = Err_UT.Average();
KErr_UT = Exts.Cov(Err_UT, Err_UT);
return(Xhat_UT);
}
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>
/// Calculates the criterion value for the estimate given the particular unscented transform parameters
/// </summary>
/// <param name="Phi">Transformation: a nonlinear function which determines the transformation of the random vector variable: y = Phi(x) + nu</param>
/// <param name="Crit">Criterion: a function which determines the quality of the unscented transform estimate. Depends on the sample covariance of the estimation error: val = Crit(Cov(X-Xhat,X-Xhat)) </param>
/// <param name="p">Unscented transform parameters</param>
/// <param name="X">Array of initial variable x samples</param>
/// <param name="Y">Array of transformed variable y = Phi(x) + nu samples</param>
/// <param name="mX">Mean of x</param>
/// <param name="KX">Cov of x</param>
/// <param name="KNu">Cov of the noize nu</param>
/// <returns>The criterion value for the particular unscented transform parameters</returns>
public static double CalculateCriterionValue(Func <Vector <double>, Vector <double> > Phi, Func <Matrix <double>, double> Crit, UTParams p, Vector <double>[] X, Vector <double>[] Y, Vector <double> mX, Matrix <double> KX, Matrix <double> KNu)
{
double crit = 0;
try
{
UnscentedTransform.Transform(x => Phi(x), mX, KX, KNu, p, out Vector <double> M_UT, out Matrix <double> Kxy_UT, out Matrix <double> Kyy_UT);
Matrix <double> P_UT = Kxy_UT * ((Kyy_UT).PseudoInverse());
Matrix <double> KErrTh_UT = KX - P_UT * Kyy_UT * P_UT.Transpose();
Vector <double>[] Xhat_UT = Y.Select(y => mX + P_UT * (y - M_UT)).ToArray();
Vector <double>[] Err_UT = Xhat_UT.Subtract(X);
Vector <double> mErr_UT = Err_UT.Average();
Matrix <double> KErr_UT = Exts.Cov(Err_UT, Err_UT);
crit = Crit(KErr_UT);//.Trace();
}
catch { crit = double.MaxValue; }
return(crit);
}
/// <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);
}