|
|
楼主 |
发表于 2010-8-8 19:19:19
|
显示全部楼层
回楼上——LIS3LV02DQ是数字接口，如果有x轴信号，芯片不太应该有问题，看看20h寄存器设置是否正确，如果设置为c1h则只有x轴工作，要三轴工作20h寄存器要设置为c7h。另外，当初为了看波形，我也使用VB6（mschart）编写过示波软件，但mschart抖动厉害，看了网上普遍存在抖动问题，加之放大缩小到功能，实在相比这个串口示波器差太远，于是放弃了。
再次附上一个用kb6mcc算法的波形——
(原文件名:sb_kb6mcc.JPG)
这个算法和先前贴出的算法同样有效，如果不会有效调节R_angle 、Q_angle和Q_gyro 的话，使用这个算法会有着更优秀的输出效果。
以下是kb6mcc算法代码——感谢kb6mcc无私奉献
//----------------------------------------------------------------
void matrix_multiply(float* A, float* B, int m, int p, int n, float* C)
{
int i, j, k;
for (i=0;i<m;i++)
for(j=0;j<n;j++)
{
C[n*i+j]=0;
for (k=0;k<p;k++)
C[n*i+j]= C[n*i+j]+A[p*i+k]*B[n*k+j];
}
}
static void matrix_addition(float* A, float* B, int m, int n, float* C)
{
int i, j;
for (i=0;i<m;i++)
for(j=0;j<n;j++)
C[n*i+j]=A[n*i+j]+B[n*i+j];
}
void matrix_subtraction(float* A, float* B, int m, int n, float* C)
{
int i, j;
for (i=0;i<m;i++)
for(j=0;j<n;j++)
C[n*i+j]=A[n*i+j]-B[n*i+j];
}
void matrix_transpose(float* A, int m, int n, float* C)
{
int i, j;
for (i=0;i<m;i++)
for(j=0;j<n;j++)
C[m*j+i]=A[n*i+j];
}
int matrix_inversion(float* A, int n, float* AInverse)
{
int i, j, iPass, imx, icol, irow;
float det, temp, pivot, factor=0;
float* ac = (float*)calloc(n*n, sizeof(float));
det = 1;
for (i = 0; i < n; i++)
{
for (j = 0; j < n; j++)
{
AInverse[n*i+j] = 0;
ac[n*i+j] = A[n*i+j];
}
AInverse[n*i+i] = 1;
}
for (iPass = 0; iPass < n; iPass++)
{
imx = iPass;
for (irow = iPass; irow < n; irow++)
{
if (fabs(A[n*irow+iPass]) > fabs(A[n*imx+iPass])) imx = irow;
}
if (imx != iPass)
{
for (icol = 0; icol < n; icol++)
{
temp = AInverse[n*iPass+icol];
AInverse[n*iPass+icol] = AInverse[n*imx+icol];
AInverse[n*imx+icol] = temp;
if (icol >= iPass)
{
temp = A[n*iPass+icol];
A[n*iPass+icol] = A[n*imx+icol];
A[n*imx+icol] = temp;
}
}
}
pivot = A[n*iPass+iPass];
det = det * pivot;
if (det == 0)
{
free(ac);
return 0;
}
for (icol = 0; icol < n; icol++)
{
AInverse[n*iPass+icol] = AInverse[n*iPass+icol] / pivot;
if (icol >= iPass) A[n*iPass+icol] = A[n*iPass+icol] / pivot;
}
for (irow = 0; irow < n; irow++)
{
if (irow != iPass) factor = A[n*irow+iPass];
for (icol = 0; icol < n; icol++)
{
if (irow != iPass)
{
AInverse[n*irow+icol] -= factor * AInverse[n*iPass+icol];
A[n*irow+icol] -= factor * A[n*iPass+icol];
}
}
}
}
free(ac);
return 1;
}
float kalman_update(float gyroscope_rate, float accelerometer_angle)
{
static float A[2][2] = {{1.0, -0.02}, {0.0, 1.0}};
static float B[2][1] = {{0.02}, {0.0}};
static float C[1][2] = {{1.0, 0.0}};
static float Sz[1][1] = {{57.5}};
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_multiply((float*) C, (float*) AxhatBu, 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];
}
///////////////////////////////////////////////////////////////////////////// |
|
发表于 2010-8-6 13:12:57
