帐号 自动登录 找回密码 密码 注册

 搜索

# 记录一段卡尔曼滤波代码

steelen 发表于 2020-6-24 11:39:09 | 显示全部楼层 |阅读模式
 ————————————————————kalman_filter.h———————————————————— /* * FileName : kalman_filter.h * Author   : xiahouzuoxin @163.com * Version  : v1.0 * Date     : 2014/9/24 20:37:01 * Brief    : * * Copyright (C) MICL,USTB */ #ifndef  _KALMAN_FILTER_H #define  _KALMAN_FILTER_H /* * NOTES: n Dimension means the state is n dimension, * measurement always 1 dimension */ /* 1 Dimension */ typedef struct {     float x;  /* state */     float A;  /* x(n)=A*x(n-1)+u(n),u(n)~N(0,q) */     float H;  /* z(n)=H*x(n)+w(n),w(n)~N(0,r)   */     float q;  /* process(predict) noise convariance */     float r;  /* measure noise convariance */     float p;  /* estimated error convariance */     float gain; } kalman1_state; /* 2 Dimension */ typedef struct {     float x[2];     /* state: [0]-angle [1]-diffrence of angle, 2x1 */     float A[2][2];  /* X(n)=A*X(n-1)+U(n),U(n)~N(0,q), 2x2 */     float H[2];     /* Z(n)=H*X(n)+W(n),W(n)~N(0,r), 1x2   */     float q[2];     /* process(predict) noise convariance,2x1 [q0,0; 0,q1] */     float r;        /* measure noise convariance */     float p[2][2];  /* estimated error convariance,2x2 [p0 p1; p2 p3] */     float gain[2];  /* 2x1 */ } kalman2_state;                   extern void kalman1_init(kalman1_state *state, float init_x, float init_p); extern float kalman1_filter(kalman1_state *state, float z_measure); extern void kalman2_init(kalman2_state *state, float *init_x, float (*init_p)[2]); extern float kalman2_filter(kalman2_state *state, float z_measure); #endif  /*_KALMAN_FILTER_H*/ ————————————————————kalman_filter.c———————————————————— /* * FileName : kalman_filter.c * Author   : xiahouzuoxin @163.com * Version  : v1.0 * Date     : 2014/9/24 20:36:51 * Brief    : * * Copyright (C) MICL,USTB */ #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 A,H,q,r 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 = {1,0}; *   and @q,@r are valued after prior tests. * *   NOTES: Please change A,H,q,r 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       = {1,0};     state->H[0]    = 1;     state->H[1]    = 0;     //state->q       = {{10e-6,0}, {0,10e-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 angle,velocity *   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 velocity. */ 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->gain[0] = temp0 / temp;     state->gain[1] = temp1 / temp;     /* x(n|n) = x(n|n-1) + gain(n) * [z_measure - H(n)*x(n|n-1)]*/     temp = state->H[0] * state->x[0] + state->H[1] * state->x[1];     state->x[0] = state->x[0] + state->gain[0] * (z_measure - temp);     state->x[1] = state->x[1] + state->gain[1] * (z_measure - temp);     /* Update @p: p(n|n) = [I - gain * H] * p(n|n-1) */     state->p[0][0] = (1 - state->gain[0] * state->H[0]) * state->p[0][0];     state->p[0][1] = (1 - state->gain[0] * state->H[1]) * state->p[0][1];     state->p[1][0] = (1 - state->gain[1] * state->H[0]) * state->p[1][0];     state->p[1][1] = (1 - state->gain[1] * state->H[1]) * state->p[1][1];     return state->x[0]; }

 您需要登录后才可以回帖 登录 | 注册 本版积分规则 回帖后跳转到最后一页

|小黑屋|手机版|Archiver|腾控科技|腾控科技 ( 京ICP备09109731号

GMT+8, 2020-10-25 07:32 , Processed in 0.110472 second(s), 22 queries .

Powered by Discuz! X3.2

© 2001-2013 Comsenz Inc.