首页 | 新闻 | 新品 | 文库 | 方案 | 视频 | 下载 | 商城 | 开发板 | 数据中心 | 座谈新版 | 培训 | 工具 | 博客 | 论坛 | 百科 | GEC | 活动 | 主题月 | 电子展
返回列表 回复 发帖

Kalman滤波器实现(4)

Kalman滤波器实现(4)

本帖最后由 look_w 于 2017-9-23 15:41 编辑

Kalman滤波C程序我就在上面公式的基础上实现了基本的Kalman滤波器,包括1维和2维状态的情况。先在头文件中声明1维和2维Kalman滤波器结构:
/* * 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*/我都给了有详细的注释,kalman1_state是状态空间为1维/测量空间1维的Kalman滤波器,kalman2_state是状态空间为2维/测量空间1维的Kalman滤波器。两个结构体都需要通过初始化函数初始化相关参数、状态值和均方差值。
/* * 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];}其实,Kalman滤波器由于其递推特性,实现起来很简单。但调参有很多可研究的地方,主要需要设定的参数如下:
  • init_x:待测量的初始值,如有中值一般设成中值(如陀螺仪)
  • init_p:后验状态估计值误差的方差的初始值
  • q:预测(过程)噪声方差
  • r:测量(观测)噪声方差。以陀螺仪为例,测试方法是:保持陀螺仪不动,统计一段时间内的陀螺仪输出数据。数据会近似正态分布,按3σ原则,取正态分布的(3σ)^2作为r的初始化值。
其中q和r参数尤为重要,一般得通过实验测试得到。
找两组声阵列测向的角度数据,对上面的C程序进行测试。一维Kalman(一维也是标量的情况,就我所知,现在网上看到的代码大都是使用标量的情况)和二维Kalman(一个状态是角度值,另一个状态是向量角度差,也就是角速度)的结果都在图中显示。这里再稍微提醒一下:状态量不要取那些能突变的量,如加速度,这点在文章“从牛顿到卡尔曼”一小节就提到过。

Matlab绘出的跟踪结果显示:
Kalman滤波结果比原信号更平滑。但是有椒盐突变噪声的地方,Kalman滤波器并不能滤除椒盐噪声的影响,也会跟踪椒盐噪声点。因此,推荐在Kalman滤波器之前先使用中值滤波算法去除椒盐突变点的影响。
上面所有C程序的源代码及测试程序都公布在我的github上,希望大家批评指正其中可能存在的错误。
返回列表