Method observe_innovation

bayes/src/infRtFlt.cpp:232–272

Bayes_base::Float Information_root_scheme::observe_innovation (Linrz_correlated_observe_model& h, const FM::Vec& s)
/* Extended linrz correlated observe
 * Precondition:
 *		r(k+1|k),R(k+1|k)
 * Postcondition:
 *		r(k+1|k+1),R(k+1|k+1)
 *
 * Uses LAPACK geqrf for QR decomposition (without PIVOTING)
 * ISSUE correctness of linrz form needs validation
 */
{
	const std::size_t x_size = x.size();
	const std::size_t z_size = s.size();
						// Size consistency, z to model
	if (z_size != h.Z.size1())
		error (Logic_exception("observation and model size inconsistent"));

						// Require Inverse of Root of uncorrelated observe noise
	UTriMatrix Zir(z_size,z_size);
	Float rcond = UCfactor (Zir, h.Z);
	rclimit.check_PD(rcond, "Z not PD");
	bool singular = UTinverse (Zir);
	assert (!singular); (void)singular;
						// Form Augmented matrix for factorisation
	DenseColMatrix A(x_size+z_size, x_size+1);	// Column major required for LAPACK, also this property is using in indexing
	A.sub_matrix(0,x_size, 0,x_size) .assign (R);
	A.sub_matrix(x_size,x_size+z_size, 0,x_size) .assign (prod(Zir, h.Hx));
	A.sub_column(0,x_size, x_size) .assign (r);
	A.sub_column(x_size,x_size+z_size, x_size) .assign (prod(Zir, s+prod(h.Hx,x)));

						// Calculate factorisation so we have and upper triangular R
	DenseVec tau(x_size+1);
	int info = LAPACK::geqrf (A, tau);
	if (info != 0)
			error (Numeric_exception("Observe no QR factor"));
						// Extract the roots, junk in strict lower triangle
	noalias(R) = UpperTri( A.sub_matrix(0,x_size, 0,x_size) );
	noalias(r) = A.sub_column(0,x_size, x_size);

	return UCrcond(R);	// compute rcond of result
}


Bayes_base::Float Information_root_scheme::observe_innovation (Linrz_uncorrelated_observe_model& h, const FM::Vec& s)