mpu6050卡尔曼滤波器程序

6轴惯性测量单元，加速度、角速度传感器MPU6050，读取数据并Kalman滤波器处理数据

/*
 * FileName : kalman_filter.c
 * Author   : xiahouzuoxin @163.com
 * Version  : v1.0
 * Date     : 2014/9/24 20:36:51
 * Brief    : 
 * 
 * Copyright （C） MICLUSTB
 */
 
#include “kalman_filter.h“

/*
 * @brief   
 *   Init fields of structure @kalman1_state.
 *   I make some defaults in this init function:
 *     A = 1;
 *     H = 1; 
 *   and @q@r are valued after prior tests.
 *
 *   NOTES: Please change AHqr according to your application.
 *
 * @inputs  
 *   state - Klaman filter structure
 *   init_x - initial x state value   
 *   init_p - initial estimated error convariance
 * @outputs 
 * @retval  
 */
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;//10e-6;  /* predict noise convariance */
    state->r = 5e2;//10e-5;  /* measure error convariance */
}

/*
 * @brief   
 *   1 Dimension Kalman filter
 * @inputs  
 *   state - Klaman filter structure
 *   z_measure - Measure value
 * @outputs 
 * @retval  
 *   Estimated result
 */
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;
}

/*
 * @brief   
 *   Init fields of structure @kalman1_state.
 *   I make some defaults in this init function:
 *     A = {{1 0.1} {0 1}};
 *     H = {10}; 
 *   and @q@r are valued after prior tests. 
 *
 *   NOTES: Please change AHqr according to your application.
 *
 * @inputs  
 * @outputs 
 * @retval  
 */
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       = {{1 0.1} {0 1}};
    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       = {{10e-60} {010e-6}};  /* measure noise convariance */
    state->q[0]    = 10e-7;
    state->q[1]    = 10e-7;
    state->r       = 10e-7;  /* estimated error convariance */
}

/*
 * @brief   
 *   2 Dimension kalman filter
 * @inputs  
 *   state - Klaman filter structure
 *   z_measure - Measure value
 * @outputs 
 *   state->x[0] - Updated state value Such as anglevelocity
 *   state->x[1] - Updated state value Such as diffrence angle acceleration
 *   state->p    - Updated estimated error convatiance matrix
 * @retval  
 *   Return value is equals to state->x[0] so maybe angle or velo

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     目录           0  2014-09-25 03:46  kalman_filter-master\
     文件         187  2014-09-25 03:46  kalman_filter-master\Makefile
     文件        2443  2014-09-25 03:46  kalman_filter-master\README.md
     文件        7775  2014-09-25 03:46  kalman_filter-master\fixed_test1.h
     文件        5790  2014-09-25 03:46  kalman_filter-master\fixed_test2.h
     文件        2067  2014-09-25 03:46  kalman_filter-master\free_test07.h
     文件        4780  2014-09-25 03:46  kalman_filter-master\kalman_filter.c
     文件        1495  2014-09-25 03:46  kalman_filter-master\kalman_filter.h
     文件        1628  2014-09-25 03:46  kalman_filter-master\plot_result.asv
     文件        1734  2014-09-25 03:46  kalman_filter-master\plot_result.m
     文件       11634  2014-09-25 03:46  kalman_filter-master\result.png
     文件        5101  2014-09-25 03:46  kalman_filter-master\result.txt
     文件        1912  2014-09-25 03:46  kalman_filter-master\test_kalman_filter.c