Method observeUD

bayes/src/UDFlt.cpp:442–511

UD_scheme::Float
 UD_scheme::observeUD (FM::Vec& gain, Float & alpha, const Vec& h, const Float r)
/** Linear UD factorisation update
 *  Bierman UdU' factorisation update. Bierman p.100
 * Input
 *  h observation coefficients
 *  r observation variance
 * Output
 *  gain  observation Kalman gain
 *  alpha observation innovation variance
 * Variables with physical significance
 *  gamma becomes covariance of innovation
 * Precondition:
 *  UD
 *  r is PSD (not checked)
 * Postcondition:
 *  UD (see return value)
 * Return:
 *  reciprocal condition number of UD, -1 if alpha singular (negative or zero)
 */
{
	std::size_t i,j,k;
	const std::size_t n = UD.size1();
	Float gamma, alpha_jm1, lamda;
	// a(n) is U'a
	// b(n) is Unweighted Kalman gain

					// Compute b = DU'h, a = U'h
	a = h;
	for (j = n-1; j >= 1; --j)	// n-1..1
	{
		for (k = 0; k < j; ++k)	// 0..j-1
		{
			a[j] += UD(k,j) * a[k];
		}
		b[j] = UD(j,j) * a[j];
	}
	b[0] = UD(0,0) * a[0];

					// Update UD(0,0), d(0) modification
	alpha = r + b[0] * a[0];
	if (alpha <= 0) goto alphaNotPD;
	gamma = 1 / alpha;
	UD(0,0) *= r * gamma;
					// Update rest of UD and gain b
	for (j = 1; j < n; ++j)		// 1..n-1
	{
					// d modification
		alpha_jm1 = alpha;	// alpha at j-1
		alpha += b[j] * a[j];
		lamda = -a[j] * gamma;
		if (alpha <= 0) goto alphaNotPD;
		gamma = 1 / alpha;
		UD(j,j) *= alpha_jm1 * gamma;
					// U modification
		for (i = 0; i < j; ++i)		// 0..j-1
		{
			Float UD_jm1 = UD(i,j);