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