|
|
ADIS16405 通过Kalman融合加速度和角速度计算出俯仰角，
由于安装原因，Z-Y两轴互换！
(原文件名:图.jpg)
红色为加速度的曲线，
蓝色为Kalman后的图形，
陀螺仪的没有打印出来，沿ADIS16405的Z轴（对调后的X轴）晃动，Kalman滤波后的数据还是跟着加速度晃动，这是什么原因！
#include <math.h>
#include <stdlib.h>
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;
float ac[10]={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.01}, {0.0, 1.0}};
static float B[2][1] = {{0.01}, {0.0}};
static float C[1][2] = {{1.0, 0.0}};
static float Sz[1][1] = {{20.0}};
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];
}
/------------------------------------------------------------------------------------/
void CalcAngle(void)
{
Sensor.AccPitch = atan2f((float)SensorBuff[ZACCL],(float)SensorBuff[XACCL])*(180/PI);
Sensor.AccRoll = atan2f((float)SensorBuff[YACCL],(float)SensorBuff[XACCL])*(180/PI);
Sensor.GyroPitch = ((float)SensorBuff[YGYRO])*0.000061;
Sensor.GyroRoll = ((float)SensorBuff[ZGYRO])*0.000061;
Sensor.Pitch = kalman_update(Sensor.GyroPitch,Sensor.AccPitch);
Sensor.Roll = kalman_update(Sensor.GyroRoll,Sensor.AccRoll);
}
int main(void)
while(1)
{
if(SensorReay==TRUE){ //中断频率为819.2Hz T=0.001220703125
SensorReay=FALSE;
GetSensorData();
CalcAngle();
if(DmaFlag==TRUE){
DmaFlag=FALSE;
SendData();
}
}
} |
|
发表于 2011-5-5 12:05:26
楼主
