#include <iostream>
#include "kalman.hpp"
using namespace std;
class kalman ax,ay,az,gx,gy,gz;
int main() {
cout<<ax.x;
return 0;
}
class kalman{
public:
float x; /* state */
float A; /* x(n)=A*x(n-1)+u(n),u(n)~N(0,q) */
float H; /* z(n)=H*x(n)+w(n),w(n)~N(0,r) */
float q; /* process(predict) noise convariance */
float r; /* measure noise convariance */
float p; /* estimated error convariance */
float gain;
float get(float z_measure);
kalman(float init_x=0,float init_p=0)
{
x = init_x;
p = init_p;
A = 1;
H = 1;
q = 2e2;//10e-6; /* predict noise convariance */
r = 5e2;//10e-5; /* measure error convariance */
}
};
float kalman::get(float z_measure)
{
/* Predict */
this->x = this->A * this->x;
this->p = this->A * this->A * this->p + this->q; /* p(n|n-1)=A^2*p(n-1|n-1)+q */
/* Measurement */
this->gain = this->p * this->H / (this->p * this->H * this->H + this->r);
this->x = this->x + this->gain * (z_measure - this->H * this->x);
this->p = (1 - this->gain * this->H) * this->p;
return this->x;
}
