一维二维卡尔曼滤波

程序描述了一维卡尔曼滤波和二维卡尔曼滤波的实现过程，其中卡尔曼滤波的初始化参数需要根据实际情况进行修改

/**
  ******************************************************************************
  * @file    kalman_filter.c
  * @author  jude jiang
  * @version V1.0.0
  * @date    31-August-2015
  * @brief   This file describes the kalman filter algorithem for the design of AHRS. 
  *  
  ******************************************************************************
  * @attention
  *
  * 
  *
  ******************************************************************************  
  */ 
#include “sys.h“
#include “kalman_filter.h“
#include “matrix_compute.h“
		
//1 dimension kalman filter init
void kalman1_init（kalman1_state *state float init_x float init_p）
{
	state->x = init_x;
	state->p = init_p;
	state->A = 1;
	state->H = 1;
	state->q = 2e2;	//predict noise covariance
	state->r = 5e2; //measure error covariance
}
	
//1 dimension kalman filter
float kalman1_filter（kalman1_state *state float z_measure）
{
	/* Predict */
	state->x = state->A * state->x;
	state->p = state->A * state->A * state->p + state->q;  /* p（n|n-1）=A^2*p（n-1|n-1）+q */

	/* Measurement */
	state->gain = state->p * state->H / （state->p * state->H * state->H + state->r）;
	state->x = state->x + state->gain * （z_measure - state->H * state->x）;
	state->p = （1 - state->gain * state->H） * state->p;

	return state->x;
}
	
//2 dimension kalman filter init
void kalman2_init（kalman2_state *state float *init_x float （*init_p）[2]）
{
	state->x[0] = init_x[0];
	state->x[1] = init_x[1];
	state->p[0][0] = init_p[0][0];
	state->p[0][1] = init_p[0][1];
	state->p[1][0] = init_p[1][0];
	state->p[1][1] = init_p[1][1];
	//state->A = {{10.1}{01}};
	state->A[0][0] = 1;
	state->A[0][1] = 0.1;
	state->A[1][0] = 0;
	state->A[1][1] = 1;
	//state->H = {10};
	state->H[0] = 1;
	state->H[1] = 0;
	
	state->q[0] = 10e-7;
	state->q[1] = 10e-7;
	state->r = 10e-7;
}

//2 dimension kalman filter
float kalman2_filter（kalman2_state *state float z_measure）
{
    float temp0 = 0.0f;
    float temp1 = 0.0f;
    float temp = 0.0f;

    /* Step1: Predict */
    state->x[0] = state->A[0][0] * state->x[0] + state->A[0][1] * state->x[1];
    state->x[1] = state->A[1][0] * state->x[0] + state->A[1][1] * state->x[1];
    /* p（n|n-1）=A^2*p（n-1|n-1）+q */
    state->p[0][0] = state->A[0][0] * state->p[0][0] + state->A[0][1] * state->p[1][0] + state->q[0];
    state->p[0][1] = state->A[0][0] * state->p[0][1] + state->A[1][1] * state->p[1][1];
    state->p[1][0] = state->A[1][0] * state->p[0][0] + state->A[0][1] * state->p[1][0];
    state->p[1][1] = state->A[1][0] * state->p[0][1] + state->A[1][1] * state->p[1][1] + state->q[1];

    /* Step2: Measurement */
    /* gain = p * H^T * [r + H * p * H^T]^（-1） H^T means transpose. */
    temp0 = state->p[0][0] * state->H[0] + state->p[0][1] * state->H[1];
    temp1 = state->p[1][0] * state->H[0] + state->p[1][1] * state->H[1];
    temp  = state->r + state->H[0] * temp0 + state->H[1] * temp1;
    state->

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     目录           0  2015-09-09 13:51  卡尔曼滤波\
     文件        3726  2015-09-07 13:44  卡尔曼滤波\kalman_filter.c
     文件        1624  2015-09-07 13:31  卡尔曼滤波\kalman_filter.h