/* Test the example of a train moving along a 1-d track */
void test_train() {
KalmanFilter f = alloc_filter(2, 1);
/* The train state is a 2d vector containing position and velocity.
Velocity is measured in position units per timestep units. */
set_matrix(f.state_transition,
1.0, 1.0,
0.0, 1.0);
/* We only observe position */
set_matrix(f.observation_model, 1.0, 0.0);
/* The covariance matrices are blind guesses */
set_identity_matrix(f.process_noise_covariance);
set_identity_matrix(f.observation_noise_covariance);
/* Our knowledge of the start position is incorrect and unconfident */
double deviation = 1000.0;
set_matrix(f.state_estimate, 10 * deviation);
set_identity_matrix(f.estimate_covariance);
scale_matrix(f.estimate_covariance, deviation * deviation);
/* Test with time steps of the position gradually increasing */
for (int i = 0; i < 10; ++i) {
set_matrix(f.observation, (double) i);
update(f);
}
/* Our prediction should be close to (10, 1) */
printf("estimated position: %f\n", f.state_estimate.data[0][0]);
printf("estimated velocity: %f\n", f.state_estimate.data[1][0]);
free_filter(f);
}
GPSFilter::GPSFilter(double noise){
//KalmanFilter alloc_filter_velocity2d(double noise) {
/* The state model has four dimensions:
x, y, x', y'
Each time step we can only observe position, not velocity, so the
observation vector has only two dimensions.
*/
f = alloc_filter(4, 2);
/* Assuming the axes are rectilinear does not work well at the
poles, but it has the bonus that we don't need to convert between
lat/long and more rectangular coordinates. The slight inaccuracy
of our physics model is not too important.
*/
double v2p = 0.001;
set_identity_matrix(f.state_transition);
set_seconds_per_timestep(1.0);
/* We observe (x, y) in each time step */
set_matrix(f.observation_model,
1.0, 0.0, 0.0, 0.0,
0.0, 1.0, 0.0, 0.0);
/* Noise in the world. */
double pos = 0.000001;
set_matrix(f.process_noise_covariance,
pos, 0.0, 0.0, 0.0,
0.0, pos, 0.0, 0.0,
0.0, 0.0, 1.0, 0.0,
0.0, 0.0, 0.0, 1.0);
/* Noise in our observation */
set_matrix(f.observation_noise_covariance,
pos * noise, 0.0,
0.0, pos * noise);
/* The start position is totally unknown, so give a high variance */
set_matrix(f.state_estimate, 0.0, 0.0, 0.0, 0.0);
set_identity_matrix(f.estimate_covariance);
double trillion = 1000.0 * 1000.0 * 1000.0 * 1000.0;
scale_matrix(f.estimate_covariance, trillion);
//return f;
}
/* Uses Gauss-Jordan elimination.
The elimination procedure works by applying elementary row
operations to our input matrix until the input matrix is reduced to
the identity matrix.
Simultaneously, we apply the same elementary row operations to a
separate identity matrix to produce the inverse matrix.
If this makes no sense, read wikipedia on Gauss-Jordan elimination.
This is not the fastest way to invert matrices, so this is quite
possibly the bottleneck. */
int destructive_invert_matrix(const Matrix_t * input, const Matrix_t * output)
{
uint8_t i,j,r;
double scalar, shear_needed;
set_identity_matrix(output);
/*
Convert input to the identity matrix via elementary row operations.
The ith pass through this loop turns the element at i,i to a 1
and turns all other elements in column i to a 0.
*/
for (i = 0; i < input->rows; ++i)
{
if (input->data[i][i] == 0.0)
{
/* We must swap rows to get a nonzero diagonal element. */
for (r = i + 1; r < input->rows; ++r)
{
if (input->data[r][i] != 0.0)
break;
}
if (r == input->rows)
/* Every remaining element in this column is zero, so this
matrix cannot be inverted. */
return 0;
swap_rows(input, i, r);
swap_rows(output, i, r);
}
/*
Scale this row to ensure a 1 along the diagonal.
We might need to worry about overflow from a huge scalar here. */
scalar = 1.0 / input->data[i][i];
scale_row(input, i, scalar);
scale_row(output, i, scalar);
/* Zero out the other elements in this column. */
for (j = 0; j < input->rows; ++j)
{
if (i == j)
continue;
shear_needed = -input->data[j][i];
shear_row(input, j, i, shear_needed);
shear_row(output, j, i, shear_needed);
}
}
return 1;
}
