invm.c++

面向对象的卡尔曼滤波器源码

/* invm.c	invm		*/

/* routines for linear systems processing via LU factorization
	from C Tools for Scientist and Engineers, L. Baker, 1989

     -- modified to generate its own workspace    EFC

     -- modified to use LU factorization in the conventional format  4/95  EFC

*/

static const char rcsid[] = "@(#)invm.c++	1.4 13:08:23 5/18/95   EFC";

#include <stdio.h>

#ifdef __TURBOC__
#include <alloc.h>
#endif
#ifdef __ZTC__
#include <stdlib.h>
#endif

// #define DEBUG

#include <lumatrix.hpp>
#include <error.hpp>

#ifdef PROFILE
#include <profile.hpp>
#endif

/* invm		invert a matrix given the LU factorization of that matrix */

Matrix invm(const LUMatrix &a)
{
#ifdef PROFILE
        Profile pr( "imvm()" );
#endif
        Error error("invm");

        if ( a.rows != a.cols )
        {
      error << "matrix inverse, not square: " << a.rows << " by " << a.cols << endl;          error.fatal();
        }
	
	int i, j, k, l, kb, kp1, kk, ii, ll;
        double sum;

	const int n = a.rows;
	const int nm1 = n - 1;

	Matrix inverse(n,n);


	inverse = 1.0;


	for (kk = 0; kk < n; kk++)
	{

        	/* solve Ly = b */
        	for (i = 0, ii = -1; i < n; i++)
        	{
                	ll = a.pivot[i];
                	sum = inverse[kk][ll];
			inverse[kk][ll] = inverse[kk][i];

                	if (ii > -1)
				for (j = ii; j < i; ++j)
					sum -= a[i][j] * inverse[kk][j];
			else if ( sum != 0.0 )
				ii = i;

			inverse[kk][i] = sum;			
        	}


        	/* solve Ux = y */
        	for (i = n-1; i >= 0; --i)
        	{
                	sum = inverse[kk][i];
			if ( i < n-1 )
                		for (j = i+1; j < n; j++)
                        		sum -= a[i][j] * inverse[kk][j];
			inverse[kk][i] = sum / a[i][i];
        	}


	}


	return inverse;

}