我来贴一个C 程序！卡尔曼滤波算法在飞行器中的应用#include <avr/io.h>
#include <avr/iom16.h>
#include <avr/interrupt.h>
#include <math.h>
#include <stdlib.h>
#include <avr/pgmspace.h>
//------------------------------------------
//-----------------------------------
#define RXB8 1
#define TXB8 0
#define UPE 2
#define OVR 3
#define FE 4
#define UDRE 5
#define RXC 7
#define FRAMING_ERROR (1<<FE)
#define PARITY_ERROR (1<<UPE)
#define DATA_OVERRUN (1<<OVR)
#define DATA_REGISTER_EMPTY (1<<UDRE)
#define RX_COMPLETE (1<<RXC)
//-----------------------------------------
volatile unsigned int datadc;
volatile unsigned char flagt0,flagt1,channel,data,adc_ready,ch0,ch1,ch2,ch3,ch4,ch5;
volatile float gyro_x,gyro_y,accel_x,accel_y,accel_z,roll,pitch,roll_rate,pitch_rate,kalman_output,kalman_output1;
//float gyro_input;
//float accel_input;
void int_all()
{
// Declare your local variables here
// Input/Output Ports initialization
// Port A initialization
// Func0=In Func1=In Func2=In Func3=In Func4=In Func5=In Func6=In Func7=In
// State0=T State1=T State2=T State3=T State4=T State5=T State6=T State7=T
PORTA=0x00;
DDRA=0x30;
// Port B initialization
// Func0=In Func1=In Func2=In Func3=In Func4=In Func5=In Func6=In Func7=In
// State0=T State1=T State2=T State3=T State4=T State5=T State6=T State7=T
PORTB=0x00;
DDRB=0x00;
// Port C initialization
// Func0=In Func1=In Func2=In Func3=In Func4=In Func5=In Func6=In Func7=In
// State0=T State1=T State2=T State3=T State4=T State5=T State6=T State7=T
PORTC=0x00;
DDRC=0x00;
// Port D initialization
// Func0=In Func1=In Func2=In Func3=In Func4=In Func5=Out Func6=In Func7=In
// State0=T State1=T State2=T State3=T State4=T State5=0 State6=T State7=T
PORTD=0x00;
DDRD=0x20;
// Timer/Counter 0 initialization
// Clock source: System Clock
// Clock value: Timer 0 Stopped
// Mode: Normal top=FFh
// OC0 output: Disconnected
TCCR0=0x00;
TCNT0=0x00;
OCR0=0x00;
// Timer/Counter 1 initialization
// Clock source: System Clock
// Clock value: 1000.000 kHz
// Mode: Normal top=FFFFh
// OC1A output: Discon.
// OC1B output: Discon.
// Noise Canceler: Off
// Input Capture on Falling Edge
TCCR1A=0x00;
TCCR1B=0x02;
TCNT1H=0x00;
TCNT1L=0x00;
OCR1AH=0x00;
OCR1AL=0x00;
OCR1BH=0x00;
OCR1BL=0x00;
// Timer/Counter 2 initialization
// Clock source: System Clock
// Clock value: Timer 2 Stopped
// Mode: Normal top=FFh
// OC2 output: Disconnected
ASSR=0x00;
TCCR2=0x00;
TCNT2=0x00;
OCR2=0x00;
// External Interrupt(s) initialization
// INT0: On
// INT0 Mode: Falling Edge
// INT1: On
// INT1 Mode: Falling Edge
// INT2: Off
GICR|=0xC0;
MCUCR=0x0A;
MCUCSR=0x00;
GIFR=0xC0;
// Timer(s)/Counter(s) Interrupt(s) initialization
TIMSK=0x04;
// USART initialization
// Communication Parameters: 8 Data, 1 Stop, No Parity
// USART Receiver: On
// USART Transmitter: On
// USART Mode: Asynchronous
// USART Baud rate: 4800
UCSRA=0x00;
UCSRB=0x98;
UCSRC=0x86;
UBRRH=0x00;
UBRRL=0x67;
// ADC initialization
// ADC Clock frequency: 125.000 kHz
// ADC Voltage Reference: AREF pin
// ADC High Speed Mode: Off
// ADC Auto Trigger Source: None
ADMUX=0;
ADCSRA=0x86;
SFIOR&=0xEF;
}
//------------------------------------------------------------------------
//uart 发送一字节
int usart_putchar(char c)
{
if(c=='\\n')
usart_putchar('\\r');
loop_until_bit_is_set(UCSRA,UDRE);
UDR=c;
return 0;
}
//uart 接收一字节
int usart_getchar(void)
{
loop_until_bit_is_set(UCSRA,RXC);
return UDR;
}
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 = (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.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];
}
//////////////////////////////////////////////////////////////////////////////
unsigned int adcread(unsigned char channel)
{
ADMUX=channel;
ADCSRA|=0x40;
while ((ADCSRA & 0x10)==0);
ADCSRA|=0x10;
ADMUX=channel;
ADCSRA|=0x40;
while ((ADCSRA & 0x10)==0);
ADCSRA|=0x10;
return ADCW;
}
//---------------------------------------------------------------------------------
SIGNAL(SIG_OVERFLOW1)
{
TCNT1=45536;
adc_ready=10;
}
//-----------------------------------------------------------
//--------------------------------------------------------------------------------------------------------
int main()
{
int aaa,bbb,ccc,ccc1,ddd;
int i,j;
char str[10];
int_all();
fdevopen(usart_putchar,usart_getchar,0);
adc_ready=0;
sei();
while(1)
{
if (adc_ready!=0)
{
adc_ready=0;
gyro_x = (float) adcread(7);
gyro_y = (float) adcread(6);
accel_x = (float) adcread(3);
accel_y = (float) adcread(2);
accel_z = (float) adcread(1);
gyro_x -= 732.3;
gyro_y -= 440.5;
accel_x -= 496.0;
accel_y -= 565.0;
accel_z -= 558.0;
roll = atan2(accel_y, accel_z);
pitch = atan2(-accel_x, accel_z);
roll_rate = gyro_x * 0.002250;
pitch_rate = gyro_y * 0.0281230;
kalman_output = kalman_update(roll_rate, roll);
aaa=roll*51;//弧度换算为角度
bbb=kalman_output*51;
//ccc=pitch*51;
//ddd=kalman_output1*51;
//------------------------------移动加权平均法
if(i<10)
{
ccc1+=pitch*51;
}
else
{
ccc1=ccc1/10;
ccc=ccc1*9/10+pitch*51/10;
i=0;
}
i++;
//-----------------------------
if(j>20)
{
//printf("加速度横滚角%d度....",aaa);
//printf("卡尔曼横滚角%d度\\n",bbb);
printf("%d.......",bbb);printf("%d\\n",ccc);
j=0;
}
j++;
}
}
}
