#include "math.h"
#include "km.h"
static double R = 1;//R; // noise power desirable
static double Q = 1;//Q; // noise power estimated
static double A = 1;//A;
static double C = 1;//C;
static double B = 0;//B;
static double K = 0;//B;
static double cov = 0;//NaN;
static double x = 0;// NaN; // estimated signal without noise
static double predX ,predCov;
double filter(double z,double u);
void reset_km(void);
void setMeasurementNoise(double noise);
void setProcessNoise(double noise);
double filter(double z,double u){
u = 0;
if (!x){
x = (1 / C) * z;
cov = (1 / C) * Q * (1 / C);
}else{
// Compute prediction
predX = (A * x) + (B * u);
predCov = ((A * cov) * A) + R;
// Kalman gain
K = predCov * C * (1 / ((C * predCov * C) + Q));
// Correction
x = predX + K * (z - (C * predX));
cov = predCov - (K * C * predCov);
}
return x;
}
void reset_km(){
R = 1;//R; // noise power desirable
Q = 1;//Q; // noise power estimated
A = 1;//A;
C = 1;//C;
B = 0;//B;
K = 0;//B;
cov = 0;//NaN;
x = 0;// NaN;
}
void setMeasurementNoise(double noise) {
Q = noise;
}
void setProcessNoise(double noise) {
R = noise;
}
