//----------------------------------------------------------------------------
void InverseLinearTransform(TPixel *res, TPixel *in,TCoor x, TCoor y,
float *conversionmatrix)
{
float inv[9];
matrix_inversion(conversionmatrix, inv);
res->a=in[12].a; /*position 12 is central position of 5x5 2D input matrix*/
res->y.i=(TChan)(inv[0] * (float)in[12].y.i +
inv[1] * (float)in[12].u.i+
inv[2] * (float)in[12].v.i + 0.5);
res->u.i=(TChan)(inv[3] * (float)in[12].y.i +
inv[4] * (float)in[12].u.i+
inv[5] * (float)in[12].v.i + 0.5);
res->v.i=(TChan)(inv[6] * (float)in[12].y.i +
inv[7] * (float)in[12].u.i+
inv[8] * (float)in[12].v.i + 0.5);
res->ax=in[12].ax;
/* limit value range from 0-255*/
if(res->y.i < 0) res->y.i=0;
if(res->y.i >255) res->y.i=255;
if(res->u.i < 0) res->u.i=0;
if(res->u.i >255) res->u.i=255;
if(res->v.i < 0) res->v.i=0;
if(res->v.i >255) res->v.i=255;
}
void kalman(float* in,float* out, float measure_noise, float process_noise)
{
float R[3][3]={0}, Q[3][3]={0};
/*const float A[3][3]= {
{1,0,0},
{0,1,0},
{0,0,1},
};
const float H[3][3]= {
{1,0,0},
{0,1,0},
{0,0,1},
}; */
const float B[3][3]= {
{1,0,0},
{0,1,0},
{0,0,1},
};
const float I[3][3]= {
{1,0,0},
{0,1,0},
{0,0,1},
};
static float x_hat[3][1];
static float P[3][3];
float z[3][1];
float x_hat_[3][1];
float P_[3][3];
float K[3][3];
float P_R1[3][3],P_R2[3][3];
float z_x_hat_[3][1],K_z_x_hat_[3][1];
float I_K[3][3];
R[0][0] = measure_noise*measure_noise;
R[1][1] = measure_noise*measure_noise;
R[2][2] = measure_noise*measure_noise;
Q[0][0] = process_noise*process_noise;
Q[1][1] = process_noise*process_noise;
Q[2][2] = process_noise*process_noise;
matrix_transpose((float*)in,1,3,(float*)z);
//--------------------------------------------------------------
// x_hat_ = A*x_hat + B*u_; <=> x_hat_ = B*x_hat
matrix_multiply((float*)B, (float*)x_hat,3,3,1,(float*)x_hat_);
//--------------------------------------------------------------
//***************************************************************
//P_ = A*P*A' + Q; <=> P_ = P + Q
matrix_addition((float*)P,(float*)Q,3,3,(float*)P_);
//***************************************************************
//..............................................................
//K = P_*H'*inv(H*P_*H' + R); <=> K = P_*inv(P_ + R)
matrix_addition((float*)P_,(float*)R,3,3,(float*)P_R1); // P_R1 = P_ + R
matrix_inversion((float*)P_R1,3,(float*)P_R2); // P_R2 = inv( P_R1)
matrix_multiply((float*)P_, (float*)P_R2,3,3,3,(float*)K);
//..............................................................
//==============================================================
//x_hat = x_hat_ + K*(z - H*x_hat_); <=> x_hat = x_hat_ + K*(z - x_hat_)
matrix_subtraction((float*)z,(float*)x_hat_,3,1,(float*)z_x_hat_); // z_x_hat_ = z - x_hat_
matrix_multiply((float*)K, (float*)z_x_hat_,3,3,1,(float*)K_z_x_hat_); // K_z_x_hat_ = K*z_x_hat_
matrix_addition((float*)x_hat_,(float*)K_z_x_hat_,3,1,(float*)x_hat); // x_hat = x_hat_ + K_z_x_hat_
//==============================================================
//::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::
//P = ( I - K*H)*P_; <=> ( I - K )*P_
matrix_subtraction((float*)I,(float*)K,3,3,(float*)I_K); // I_K = I - K
matrix_multiply((float*)I_K, (float*)P_,3,3,3,(float*)P); // P = I_K*P_
//::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::
matrix_transpose((float*)x_hat,3,1,(float*)out);
}
float kalman_update(float gyroscope_rate, float accelerometer_angle)
{
static float A[2][2] = {{1.0, -0.019968}, {0.0, 1.0}};
static float B[2][1] = {{0.019968}, {0.0}};
static float C[1][2] = {{1.0, 0.0}};
static float Sz[1][1] = {{17.2}};
static float Sw[2][2] = {{0.005, 0.005}, {0.005, 0.005}};
static float xhat[2][1] = {{0.0}, {0.0}};
static float P[2][2] = {{0.005, 0.005}, {0.005, 0.005}};
float u[1][1];
float y[1][1];
float AP[2][2];
float CT[2][1];
float APCT[2][1];
float CP[1][2];
float CPCT[1][1];
float CPCTSz[1][1];
float CPCTSzInv[1][1];
float K[2][1];
float Cxhat[1][1];
float yCxhat[1][1];
float KyCxhat[2][1];
float Axhat[2][1];
float Bu[2][1];
float AxhatBu[2][1];
float AT[2][2];
float APAT[2][2];
float APATSw[2][2];
float KC[2][2];
float KCP[2][2];
float KCPAT[2][2];
u[0][0] = gyroscope_rate;
y[0][0] = accelerometer_angle;
matrix_multiply((float*) A, (float*) xhat, 2, 2, 1, (float*) Axhat);
matrix_multiply((float*) B, (float*) u, 2, 1, 1, (float*) Bu);
matrix_addition((float*) Axhat, (float*) Bu, 2, 1, (float*) AxhatBu);
matrix_multiply((float*) C, (float*) xhat, 1, 2, 1, (float*) Cxhat);
matrix_subtraction((float*) y, (float*) Cxhat, 1, 1, (float*) yCxhat);
matrix_transpose((float*) C, 1, 2, (float*) CT);
matrix_multiply((float*) C, (float*) P, 1, 2, 2, (float*) CP);
matrix_multiply((float*) CP, (float*) CT, 1, 2, 1, (float*) CPCT);
matrix_addition((float*) CPCT, (float*) Sz, 1, 1, (float*) CPCTSz);
matrix_multiply((float*) A, (float*) P, 2, 2, 2, (float*) AP);
matrix_multiply((float*) AP, (float*) CT, 2, 2, 1, (float*) APCT);
matrix_inversion((float*) CPCTSz, 1, (float*) CPCTSzInv);
matrix_multiply((float*) APCT, (float*) CPCTSzInv, 2, 1, 1, (float*) K);
matrix_multiply((float*) K, (float*) yCxhat, 2, 1, 1, (float*) KyCxhat);
matrix_addition((float*) AxhatBu, (float*) KyCxhat, 2, 1, (float*) xhat);
matrix_transpose((float*) A, 2, 2, (float*) AT);
matrix_multiply((float*) AP, (float*) AT, 2, 2, 2, (float*) APAT);
matrix_addition((float*) APAT, (float*) Sw, 2, 2, (float*) APATSw);
matrix_multiply((float*) K, (float*) C, 2, 1, 2, (float*) KC);
matrix_multiply((float*) KC, (float*) P, 2, 2, 2, (float*) KCP);
matrix_multiply((float*) KCP, (float*) AT, 2, 2, 2, (float*) KCPAT);
matrix_subtraction((float*) APATSw, (float*) KCPAT, 2, 2, (float*) P);
return xhat[0][0];
}
