/* -*- indent-tabs-mode:T; c-basic-offset:8; tab-width:8; -*- vi: set ts=8:
 * $Id: kalman.c,v 1.5 2002/02/15 18:11:57 tramm Exp $
 *
 * This is the core Kalman filtering algorithm.
 *
 * Author: Aaron Kahn, Suresh Kannan, Eric Johnson
 * copyright 2001
 *
 * Merged into the Autopilot project by Trammell Hudson <hudson@swcp.com>
 */

#include <math.h>
#include <string.h>

#include "kalman.h"


/*
 * This will perform a Kalman filter Gain, state, and coveriance
 * matrix update.  What is needed is the linear A matrix, state vector,
 * C matrix, P coveriance matrix, measurement vector, and the R matrix.
 *
 *	A(n,n)		Linear model
 *	P(n,n)		Coveriance matrix
 *	X(n,1)		State Vector
 *	C(m,n)		Measurement matrix; m=# of measurements, n=# of states
 *	R(m,n)		Measurement weight matrix 
 *	ys(m,1)		Measurement vector
 *	K(n,m)		Kalman Gain matrix
 */
void
kalmanUpdate(
	Matrix			A,
	Matrix			P,
	Vector			X,
	Matrix			C,
	Matrix			R,
	Vector			ys,
	Matrix			K,
	int			n,
	int			m
)
{
	Matrix			E;
	Matrix			T1;
	Matrix			T2;
	Vector			TV1;
	Vector			TV2;
	
	/* perform E = C*P*C' + R */
	MMmult(C,P,T1,m,n,n);
	transpose(C,T2,m,n);
	MMmult(T1,T2,E,m,n,m);
	MMadd(E,R,E,m,m);
	
	/* perform K = P*C'*inv(E).  T2 still has C' */
	MMmult(P,T2,T1,n,n,m);
	inv(E,T2,m);
	MMmult(T1,T2,K,n,m,m);
	
	/* perform x = x + K*( ys - yp ) */
	MVmult(C,X,TV1,m,n);
	VVsub( ys, TV1, TV1, m );
	MVmult(K,TV1,TV2,n,m);
	VVadd(X,TV2,X,n);
	
	
	/* perform P = P - K*C*P */
	MMmult(K,C,T1,n,m,n);
	MMmult(T1,P,T2,n,n,n);
	MMsub( P, T2, P, n, n );
}