婷婷五月国产人综合在线,91九色自拍视频,gogogo高清在线观看中文版

/* kalman.c
This file contains the code for a kalman filter, an
extended kalman filter, and an iterated extended kalman filter.
For ready extensibility, the apply_measurement() and apply_system()
functions are located in a separate file: kalman_cam.c is an example.
It uses the matmath functions provided in an accompanying file
to perform matrix and quaternion manipulation.
J. Watlington, 11/15/95
Modified:
11/30/95 wad The extended kalman filter section seems to be
working now.
*/
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include "kalman.h"
#define ITERATION_THRESHOLD 2.0
#define ITERATION_DIVERGENCE 20
/* The following are the global variables of the Kalman filters,
used to point to data structures used throughout. */
static m_elem *state_pre; /* ptr to apriori state vectors, x(-) */
static m_elem *state_post; /* ptr to aposteriori state vectors, x(+) */
static m_elem *iter_state0;
static m_elem *iter_state1;
static m_elem **cov_pre; /* ptr to apriori covariance matrix, P(-) */
static m_elem **cov_post; /* ptr to apriori covariance matrix, P(-) */
static m_elem **sys_noise_cov; /* system noise covariance matrix (GQGt) */
static m_elem **mea_noise_cov; /* measurement noise variance vector (R) */
static m_elem **sys_transfer; /* system transfer function (Phi) */
static m_elem **mea_transfer; /* measurement transfer function (H) */
static m_elem **kalman_gain; /* The Kalman Gain matrix (K) */
int global_step = 0; /* the current step number (k) */
int measurement_size; /* number of elems in measurement */
int state_size; /* number of elements in state */
/* Temporary variables, declared statically to avoid lots of run-time
memory allocation. */
static m_elem *z_estimate; /* a measurement_size x 1 vector */
static m_elem **temp_state_state; /* a state_size x state_size matrix */
static m_elem **temp_meas_state; /* a measurement_size x state_size matrix */
static m_elem **temp_meas_meas; /* a measurement_size squared matrix */
static m_elem **temp_meas_2; /* another one ! */
/* Prototypes of internal functions */
static void alloc_globals( int num_state,
int num_measurement );
static void update_system( m_elem *z, m_elem *x_minus,
m_elem **kalman_gain, m_elem *x_plus );
static void estimate_prob( m_elem **P_post, m_elem **Phi, m_elem **GQGt,
m_elem **P_pre );
static void update_prob( m_elem **P_pre, m_elem **R, m_elem **H,
m_elem **P_post, m_elem **K );
static void take_inverse( m_elem **in, m_elem **out, int n );
static m_elem calc_state_change( m_elem *a, m_elem *b );
/******************************************************************
Linear Kalman Filtering
kalman_init()
This function initializes the kalman filter. Note that for a
straight-forward (linear) Kalman filter, this is the only place that
K and P are computed... */
void kalman_init( m_elem **GQGt, m_elem **Phi, m_elem **H, m_elem **R,
m_elem **P, m_elem *x, int num_state, int num_measurement )
{
alloc_globals( num_state, num_measurement );
/* Init the global variables using the arguments. */
vec_copy( x, state_post, state_size );
mat_copy( P, cov_post, state_size, state_size );
sys_noise_cov = GQGt;
mea_noise_cov = R;
sys_transfer = Phi;
mea_transfer = H;
/***************** Gain Loop *****************/
estimate_prob( cov_post, sys_transfer, sys_noise_cov, cov_pre );
update_prob( cov_pre, mea_noise_cov, mea_transfer, cov_post, kalman_gain );
}
/* kalman_step()
This function takes a set of measurements, and performs a single
recursion of the straight-forward kalman filter.
*/
void kalman_step( m_elem *z_in )
{
/************** Estimation Loop ***************/
apply_system( state_post, state_pre );
update_system( z_in, state_pre, kalman_gain, state_post );
global_step++;
}
/* kalman_get_state
This function returns a pointer to the current estimate (a posteriori)
of the system state. */
m_elem *kalman_get_state( void )
{
return( state_post );
}
/******************************************************************
Non-linear Kalman Filtering
extended_kalman_init()
This function initializes the extended kalman filter.
*/
void extended_kalman_init( m_elem **GQGt, m_elem **R, m_elem **P, m_elem *x,
int num_state, int num_measurement )
{
#ifdef PRINT_DEBUG
printf( "ekf: Initializing filter\n" );
#endif
alloc_globals( num_state, num_measurement );
sys_transfer = matrix( 1, num_state, 1, num_state );
mea_transfer = matrix( 1, num_measurement, 1, num_state );
/* Init the global variables using the arguments. */
vec_copy( x, state_post, state_size );
vec_copy( x, state_pre, state_size );
mat_copy( P, cov_post, state_size, state_size );
mat_copy( P, cov_pre, state_size, state_size );
sys_noise_cov = GQGt;
mea_noise_cov = R;
}
/* extended_kalman_step()
This function takes a set of measurements, and performs a single
recursion of the extended kalman filter.
*/
void extended_kalman_step( m_elem *z_in )
{
#ifdef PRINT_DEBUG
printf( "ekf: step %d\n", global_step );
#endif
/***************** Gain Loop *****************
First, linearize locally, then do normal gain loop */
generate_system_transfer( state_pre, sys_transfer );
generate_measurement_transfer( state_pre, mea_transfer );
estimate_prob( cov_post, sys_transfer, sys_noise_cov, cov_pre );
update_prob( cov_pre, mea_noise_cov, mea_transfer, cov_post, kalman_gain );
/************** Estimation Loop ***************/
apply_system( state_post, state_pre );
update_system( z_in, state_pre, kalman_gain, state_post );
global_step++;
}
/* iter_ext_kalman_init()
This function initializes the iterated extended kalman filter
*/
void iter_ext_kalman_init( m_elem **GQGt, m_elem **R, m_elem **P, m_elem *x,
int num_state, int num_measurement )
{
#ifdef PRINT_DEBUG
printf( "iekf: Initializing filter\n" );
#endif
alloc_globals( num_state, num_measurement );
iter_state0 = vector( 1, num_state );
iter_state1 = vector( 1, num_state );
sys_transfer = matrix( 1, num_state, 1, num_state );
mea_transfer = matrix( 1, num_measurement, 1, num_state );
/* Init the global variables using the arguments. */
vec_copy( x, state_post, state_size );
vec_copy( x, state_pre, state_size );
mat_copy( P, cov_post, state_size, state_size );
mat_copy( P, cov_pre, state_size, state_size );
sys_noise_cov = GQGt;
mea_noise_cov = R;
}
/* iter_ext_kalman_step()
This function takes a set of measurements, and iterates over a single
recursion of the extended kalman filter.
*/
void iter_ext_kalman_step( m_elem *z_in )
{
int iteration = 1;
m_elem est_change;
m_elem *prev_state;
m_elem *new_state;
m_elem *temp;
generate_system_transfer( state_pre, sys_transfer );
estimate_prob( cov_post, sys_transfer, sys_noise_cov, cov_pre );
apply_system( state_post, state_pre );
/* Now iterate, updating the probability and the system model
until no change is noticed between iteration steps */
prev_state = iter_state0;
new_state = iter_state1;
generate_measurement_transfer( state_pre, mea_transfer );
update_prob( cov_pre, mea_noise_cov, mea_transfer,
cov_post, kalman_gain );
update_system( z_in, state_pre, kalman_gain, prev_state );
est_change = calc_state_change( state_pre, prev_state );
while( (est_change < ITERATION_THRESHOLD) &&

復制代碼