卡尔曼滤波(一阶和二阶)
来源:互联网 发布:淘宝店铺降权多久恢复 编辑:程序博客网 时间:2024/05/18 03:23
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];}
0 0
- 卡尔曼滤波(一阶和二阶)
- 一阶互补滤波,二阶互补滤波,卡尔曼滤波
- MPU6050 + 一阶互补滤波+二阶互补滤波+卡尔曼滤波 +波形比较
- MPU6050对卡尔曼、一阶互补滤波、二阶互补滤波的比较
- MPU6050对卡尔曼、一阶互补滤波、二阶互补滤波的比较
- 谈谈MPU6050的数据融合 一阶滤波 卡尔曼滤波
- 卡尔曼滤波之Opencv(二)
- 卡尔曼滤波和最小二乘估计的结合
- 粒子滤波和卡尔曼滤波简介
- 维纳滤波和卡尔曼滤波
- 卡尔曼滤波和维纳滤波
- 卡尔曼滤波(KF)和扩展卡尔曼滤波(EKF)
- 无人驾驶汽车系统入门(二)——高级运动模型和扩展卡尔曼滤波
- 卡尔曼滤波(程序)
- 卡尔曼滤波(一)
- 卡尔曼滤波(2)
- 卡尔曼滤波原理二:扩展卡尔曼
- 卡尔曼(Kalman)滤波(二)--Kalman滤波的发展历史与应用
- linux删除普通用户报错 userdel: user fancy is currently used by process 1973
- 分布式 Key-Value 存储系统:Cassandra 入门
- Java8-流-简介
- KVM虚拟机
- 13 函数模板与类模板
- 卡尔曼滤波(一阶和二阶)
- Windows下Python多版本共存 (2.7 3.5)
- Navicat、SQLyog导入数据库遇到的问题
- Dubbo详细介绍与安装使用过程
- CentOs开放需要的端口
- Vue.js——60分钟组件快速入门(下篇)
- javascript篇二
- Jdbc调用Sqlserver各版本
- 动脑学院高级ui效果之水波纹