static double update(gsl_matrix *v, gsl_matrix *w, gsl_matrix *h)
{
double dist = 0;
gsl_matrix *wt=NULL, *ht=NULL, *wh=NULL;
gsl_matrix *w_h=NULL, *wt_w=NULL;
gsl_matrix *wt_v = NULL;
gsl_matrix *v_ht=NULL, *wt_w_h=NULL, *w_h_ht=NULL;
wt = gsl_matrix_alloc(w->size2, w->size1);
gsl_matrix_transpose_memcpy(wt, w);
ht = gsl_matrix_alloc(h->size2, h->size1);
gsl_matrix_transpose_memcpy(ht, h);
// wt * v
wt_v = mm(wt, v);
// wt * w * h
wt_w = mm(wt, w);
wt_w_h = mm(wt_w, h);
gsl_matrix_free(wt_w);
// h = h.mul_elements(wt * v).div_elements(wt * w * h)
gsl_matrix_mul_elements(h, wt_v);
gsl_matrix_div_elements(h, wt_w_h);
gsl_matrix_free(wt_v);
gsl_matrix_free(wt_w_h);
// v * ht
v_ht = mm(v, ht);
// w * h * ht
w_h = mm(w, h);
w_h_ht = mm(w_h, ht);
gsl_matrix_free(w_h);
// w = w.mul_elements(v * ht).div_elements(w * h * ht)
gsl_matrix_mul_elements(w, v_ht);
gsl_matrix_div_elements(w, w_h_ht);
gsl_matrix_free(v_ht);
gsl_matrix_free(w_h_ht);
gsl_matrix_free(wt);
gsl_matrix_free(ht);
wh = mm(w, h);
dist = difcost(v, wh);
gsl_matrix_free(wh);
// w and h were modified in place
return dist;
}
int ratio_with_sd(gsl_matrix *A, gsl_matrix *sdA, gsl_matrix *B, gsl_matrix *sdB){
/* we take the ratio A./B (by elements) B is repeated to match the
* size of A A must have the same number of columns and an integer
* multiple of rows, e.g. A=[B;B;B]. the result is stored in A.
* All matrices are expected to contain only positive elements.
*/
int l;
int C,T,F;
gsl_matrix_view a;
gsl_matrix *R; // for the relative error of B
T=B->size1;
C=(A->size1)/T;
F=A->size2;
// printf("sizes: T=%i, C=%i, F=%i\n",T,C,F);
R=gsl_matrix_alloc(C*T,F);
for (l=0;l<C;l++){
a=gsl_matrix_submatrix(A,l*T,0,T,F);
gsl_matrix_div_elements(&(a.matrix),B); //A'=A./B
}
for (l=0;l<C;l++){
a=gsl_matrix_submatrix(sdA,l*T,0,T,F);//sdA'=sdA./B
gsl_matrix_div_elements(&(a.matrix),B);
}
gsl_matrix_div_elements(sdB,B); //sdB'=sdB./B
gsl_matrix_memcpy(R,A); // copy: R=A'=A./B
for (l=0;l<C;l++){
a=gsl_matrix_submatrix(R,l*T,0,T,F);
gsl_matrix_mul_elements(&(a.matrix),sdB); //R effectively ends up being A./B*sdB/B
}
gsl_matrix_add(sdA,R);
gsl_matrix_free(R);
return EXIT_SUCCESS;
}
void kalmanFilterPosition0(double latitude, double longitude, gsl_matrix *x, gsl_matrix *P)
{
/* Wektor pomiarów */
gsl_matrix *z = gsl_matrix_alloc(2, 1);
gsl_matrix_set(z, 0, 0, latitude);
gsl_matrix_set(z, 1, 0, longitude);
/* Macierz przejœcia ze stanu do pomiaru */
gsl_matrix *H = gsl_matrix_alloc(2,2);
gsl_matrix_set_identity(H);
/* Macierz kowariancji szumu procesu */
gsl_matrix *Q = gsl_matrix_alloc(2,2);
gsl_matrix_set_identity(Q);
double q = 0.00001;
gsl_matrix_scale(Q, q);
/* Macierz kowariancji szumu pomiaru */
double wrong_latitude = pow(WRONG_LATI, 2);
double wrong_longitude = pow(WRONG_LONGI,2);
gsl_matrix *R = gsl_matrix_calloc(2,2);
gsl_matrix_set(R, 0, 0, wrong_latitude);
gsl_matrix_set(R, 1, 1, wrong_longitude);
/* Macierz jednostkowa */
gsl_matrix *I = gsl_matrix_alloc(2,2);
gsl_matrix_set_identity(I);
/* P = P(k-1) + Q */
gsl_matrix_add(P, Q);
/* K = P/(P + R) */
gsl_matrix *K = gsl_matrix_calloc(2,2);
(*K) = (*P);
gsl_matrix_add(R, P);
gsl_matrix_div_elements(K, R);
/* P(k) = (I-K)*P */
gsl_matrix_sub(I, K);
gsl_matrix_mul_elements(I, P);
(*P) = (*I);
/* x(k) = x + K(z-x) */
gsl_matrix_sub(z, x);
gsl_matrix_mul_elements(K, z);
gsl_matrix_add(x, K); //zaktualizowana wartoœc
gsl_matrix_free(z);
gsl_matrix_free(H);
gsl_matrix_free(Q);
gsl_matrix_free(R);
gsl_matrix_free(I);
gsl_matrix_free(K);
}
int main() {
srand(time(NULL));
int states = 6;
int unfiltered_states = 3;
gsl_matrix *state_mean = gsl_matrix_calloc(states,1);
gsl_matrix_set(state_mean, 0,0, 3);
gsl_matrix_set(state_mean, 1,0, 3);
gsl_matrix *state_covariance = gsl_matrix_calloc(states,states);
gsl_matrix *observation_mean = gsl_matrix_calloc(states,1);
gsl_matrix *observation_covariance = gsl_matrix_calloc(states,states);
//gsl_matrix_set(observation_covariance, 0, 0, hdop);
//gsl_matrix_set(observation_covariance, 1, 1, hdop);
//gsl_matrix_set(observation_covariance, 2, 2, vert_err);
//gsl_matrix_set(observation_covariance, 3, 3, (2*hdop)/pow(timestep,2));
//gsl_matrix_set(observation_covariance, 4, 4, (2*hdop)/pow(timestep,2));
//gsl_matrix_set(observation_covariance, 5, 5, (2*vert_err)/pow(timestep,2));
//gsl_matrix_set(observation_covariance, 0, 3, timestep*(2*hdop)/pow(timestep,2));
//gsl_matrix_set(observation_covariance, 3, 0, timestep*(2*hdop)/pow(timestep,2));
//gsl_matrix_set(observation_covariance, 1, 4, timestep*(2*hdop)/pow(timestep,2));
//gsl_matrix_set(observation_covariance, 4, 1, timestep*(2*hdop)/pow(timestep,2));
//gsl_matrix_set(observation_covariance, 2, 5, timestep*(2*vert_err)/pow(timestep,2));
//gsl_matrix_set(observation_covariance, 5, 2, timestep*(2*vert_err)/pow(timestep,2));
gsl_matrix *observation_transformation = gsl_matrix_calloc(states,states);
gsl_matrix *estimate_mean = gsl_matrix_calloc(states,1);
gsl_matrix *estimate_covariance = gsl_matrix_calloc(states,states);
gsl_matrix *kalman_gain = gsl_matrix_calloc(states,states);
gsl_matrix *temp21a = gsl_matrix_calloc(states,1);
gsl_matrix *temp21b = gsl_matrix_calloc(states,1);
gsl_matrix *temp22a = gsl_matrix_calloc(states,states);
gsl_matrix *temp22b = gsl_matrix_calloc(states,states);
gsl_matrix *predict = gsl_matrix_calloc(states,states);
//gsl_matrix_set(predict, 0, 0, 1);
//gsl_matrix_set(predict, 1, 1, 1);
//gsl_matrix_set(predict, 2, 2, 1);
gsl_matrix_set(predict, 3, 3, 1);
gsl_matrix_set(predict, 4, 4, 1);
gsl_matrix_set(predict, 5, 5, 1);
//gsl_matrix_set(predict, 0, 3, timestep);
//gsl_matrix_set(predict, 1, 4, timestep);
//gsl_matrix_set(predict, 2, 5, timestep);
gsl_matrix *control = gsl_matrix_calloc(states, unfiltered_states);
//gsl_matrix_set(control, 0, 0, 0.5*pow(timestep,2));
//gsl_matrix_set(control, 1, 1, 0.5*pow(timestep,2));
//gsl_matrix_set(control, 2, 2, 0.5*pow(timestep,2));
//gsl_matrix_set(control, 3, 0, timestep);
//gsl_matrix_set(control, 4, 1, timestep);
//gsl_matrix_set(control, 5, 2, timestep);
gsl_vector *acceleration = gsl_vector_calloc(unfiltered_states);
gsl_vector *velocity = gsl_vector_calloc(unfiltered_states);
gsl_vector *location = gsl_vector_calloc(unfiltered_states);
gsl_vector *delta_location = gsl_vector_calloc(unfiltered_states);
gsl_vector *temp_location = gsl_vector_calloc(unfiltered_states);
double timestep;
int hdop;
int vert_err;
int s;
double error;
for (int t = 1; t < 1000000; t++) {
timestep = 1;
gsl_vector_set(acceleration, 0, (rand()%101)-52);
gsl_vector_set(acceleration, 1, (rand()%101)-46);
gsl_vector_set(acceleration, 2, (rand()%101)-54);
gsl_vector_add(velocity, acceleration);
gsl_vector_add(location, velocity);
gsl_vector_memcpy(delta_location, location);
gsl_vector_sub(delta_location, temp_location);
gsl_vector_memcpy(temp_location, location);
gsl_matrix_set(predict, 0, 3, timestep);
gsl_matrix_set(predict, 1, 4, timestep);
gsl_matrix_set(predict, 2, 5, timestep);
gsl_matrix_set(control, 0, 0, 0.5*pow(timestep,2));
gsl_matrix_set(control, 1, 1, 0.5*pow(timestep,2));
gsl_matrix_set(control, 2, 2, 0.5*pow(timestep,2));
gsl_matrix_set(control, 3, 0, timestep);
gsl_matrix_set(control, 4, 1, timestep);
gsl_matrix_set(control, 5, 2, timestep);
hdop = 5;
vert_err = 5;
gsl_matrix_set(observation_covariance, 0, 0, hdop);
gsl_matrix_set(observation_covariance, 1, 1, hdop);
gsl_matrix_set(observation_covariance, 2, 2, vert_err);
gsl_matrix_set(observation_covariance, 3, 3, (2*hdop)/pow(timestep,2));
gsl_matrix_set(observation_covariance, 4, 4, (2*hdop)/pow(timestep,2));
gsl_matrix_set(observation_covariance, 5, 5, (2*vert_err)/pow(timestep,2));
gsl_matrix_set(observation_covariance, 0, 3, timestep*(2*hdop)/pow(timestep,2));
gsl_matrix_set(observation_covariance, 3, 0, timestep*(2*hdop)/pow(timestep,2));
gsl_matrix_set(observation_covariance, 1, 4, timestep*(2*hdop)/pow(timestep,2));
gsl_matrix_set(observation_covariance, 4, 1, timestep*(2*hdop)/pow(timestep,2));
gsl_matrix_set(observation_covariance, 2, 5, timestep*(2*vert_err)/pow(timestep,2));
gsl_matrix_set(observation_covariance, 5, 2, timestep*(2*vert_err)/pow(timestep,2));
//observation_transformation = observation_mean[k] * pseudoinverse(state_mean[k-1])
gsl_matrix_set_zero(temp21a);
gsl_matrix_set(temp21a, 0, 0, gsl_vector_get(delta_location,0));
gsl_matrix_set(temp21a, 1, 0, gsl_vector_get(delta_location,1));
gsl_matrix_set(temp21a, 2, 0, gsl_vector_get(delta_location,2));
gsl_matrix_set(temp21a, 3, 0, gsl_vector_get(velocity,0));
gsl_matrix_set(temp21a, 4, 0, gsl_vector_get(velocity,1));
gsl_matrix_set(temp21a, 5, 0, gsl_vector_get(velocity,2));
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1, temp21a, pseudo_inverse(state_mean), 0, observation_transformation);
//observation_mean[k] = observation_transformation * state_mean[k-1]
gsl_matrix_memcpy(temp21a, state_mean);
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1, observation_transformation, temp21a, 0, observation_mean);
//observation_covariance[k] = observation_transformation * state_covariance * transpose(observation_transformation)
/* gsl_matrix_set_zero(temp22a);
gsl_blas_dgemm(CblasNoTrans, CblasTrans, 1, state_covariance, observation_transformation, 0, temp22a); //notice observation_transformation is transposed
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1, observation_transformation, temp22a, 0, observation_covariance);
*/
gsl_matrix_set(observation_covariance, 0, 3, timestep*3);
gsl_matrix_set(observation_covariance, 3, 0, timestep*3);
gsl_matrix_set(observation_covariance, 1, 4, timestep*5);
gsl_matrix_set(observation_covariance, 4, 1, timestep*5);
gsl_matrix_set(observation_covariance, 2, 5, timestep*6);
gsl_matrix_set(observation_covariance, 5, 2, timestep*6);
//estimate_mean = predict * state_mean + control * acceleration;
gsl_matrix_set_zero(estimate_mean);
gsl_matrix_set_zero(temp21a);
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1, predict, state_mean, 0, estimate_mean);
gsl_matrix_view test = gsl_matrix_view_vector(acceleration, unfiltered_states, 1);
gsl_matrix * tmp = &test.matrix;
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1, control, tmp, 0, temp21a);
gsl_matrix_add(estimate_mean, temp21a);
//estimate_covariance = predict * state_covariance * transpose(predict) + noise;
gsl_matrix_set_zero(estimate_covariance);
gsl_matrix_set_zero(temp22a);
gsl_blas_dgemm(CblasNoTrans, CblasTrans, 1, state_covariance, predict, 0, temp22a); //notice predict is transposed
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1, predict, temp22a, 0, estimate_covariance);
gsl_matrix_add_constant(estimate_covariance, 0.1); //adxl345 noise, this is completely wrong
//kalman_gain = estimate_covariance * pseudoinverse(estimate_covariance + observation_covariance);
gsl_matrix_set_zero(kalman_gain);
gsl_matrix_set_zero(temp22a);
gsl_matrix_memcpy(temp22a, observation_covariance);
gsl_matrix_add(temp22a, estimate_covariance);
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1, estimate_covariance, pseudo_inverse(temp22a), 0, kalman_gain);
//state_mean = estimate_mean + kalman_gain * ( observation_mean - estimate_mean );
gsl_matrix_set_zero(state_mean);
gsl_matrix_set_zero(temp21a);
gsl_matrix_memcpy(temp21a, observation_mean);
gsl_matrix_sub(temp21a, estimate_mean);
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1, kalman_gain, temp21a, 0, state_mean);
gsl_matrix_add(state_mean, estimate_mean);
//state_covariance = estimate_covariance - kalman_gain * ( estimate_covariance );
gsl_matrix_set_zero(state_covariance);
gsl_matrix_set_zero(temp22a);
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1, kalman_gain, estimate_covariance, 0, temp22a);
gsl_matrix_add(state_covariance, estimate_covariance);
gsl_matrix_sub(state_covariance, temp22a);
printf("state_mean:");
gsl_matrix_fprintf(stdout, state_mean, "%f");
//printf("state_covariance:");
//gsl_matrix_fprintf(stdout, state_covariance, "%f");
printf("observation_mean:");
gsl_matrix_fprintf(stdout, observation_mean, "%f");
//printf("observation_covariance:");
//gsl_matrix_fprintf(stdout, observation_covariance, "%f");
printf("estimate_mean:");
gsl_matrix_fprintf(stdout, estimate_mean, "%f");
//printf("estimate_covariance:");
//gsl_matrix_fprintf(stdout, estimate_covariance, "%f");
gsl_matrix_set_zero(temp21a);
gsl_matrix_memcpy(temp21a, observation_mean);
gsl_matrix_sub(temp21a, state_mean);
gsl_matrix_div_elements(temp21a, observation_mean);
gsl_matrix_mul_elements(temp21a, temp21a);
for (int i = states-1; i >= 0; i--) {
error += gsl_matrix_get(temp21a, i, 0);
}
printf("error: %f\n", error);
printf("error/time: %f\n", error/t);
printf("\n");
usleep(1000000);
}
}