卡尔曼滤波——包含C/C++实现方式及MPU6050实例
2023-04-18 16:13:42 时间
1.简介
简单的来说卡尔曼滤波的核心思想是：假定我们需要观测的系统输出是线性的，而噪声都满足高斯分布。这一刻系统的状态（最优估计）是这一刻的预测值和这一刻的测量值的加权平均，当得到最优估计之后，再将这一刻的最优估计和估计值进行对比，如果相差比较小，则说明估计比较准确，下次计算就加大估计值的权值，否则说明估计值不准确，下次计算就加大测量值的权值，循环以上过程，进行动态调整。
测量一个实际的参数时，可以根据以前的数据预测下一个数据的走向，我们把这个数作为预测值,与此同时还有一个传感器可以测量到这个参数，我们就把它作为测量值。如果对测量值和预测值的可信度没有把握就可以对半开，表示为下面这样：
输出值 = 0.5 * 测量值 + 0.5 * 预测值
如果这个测量值噪声有点大，不太可信，那就三七开吧，相信预测值多一点：
输出值 = 0.3 * 测量值 + 0.7 * 预测值
上面的式子换个写法的话：
输出值 = 0.3 * 测量值 + （1-0.3） * 预测值
= 预测值 + 0.3 *（测量值 - 预测值）
= 预测值 + K *（测量值 - 预测值）
这样我们用一个参数K就可以表示我们对数据的可信度（参数K越大，表示测量值越可信），我们总是希望输出值可信度越高越好，也就是不确定度越小越好。测量值和观测值都有不确定度，而卡尔曼滤波算法就是基于数值的不确定性调节这个参数K（卡尔曼增益）。在迭代不定次数后接近输出值不确定性最小的数值。
说实话很多时候对理论的东西不需要有太深的了解，主要是会用就行了，所以下面有大家用起来比较方便的的C和C++的版本代码，也有我实际做的MPU6050的输出滤波（C），是从工程当中抠出来的，但除了一些特定环境下的设置，算法部分是可以直接拿去用的。关于配置我也有一些建议：
看下面的代码可能大家就会发现，估计噪声和测量噪声的方差是需要自己配置的，在实际工程当中中，这些数值可能得需要大家实测，但是我们可以从推导公式中发现，只要固定估计噪声和测量噪声的方差的比值，计算的结果就是一样的。实际上，我们在实际测试滤波效果的时候，调整比值就可以改变滤波的效果，这个比值越大，说明测量值越不可靠。
开始的时候还需要提供估计值的初值，这时最好把协方差矩阵的初始值设为非零数，也就是开始的状态基于测量值。（当然，如果你有非常可靠的状态初值，建议把协方差矩阵初始值设为零）
2.C语言版本
其中包含一维数据和二维数据的滤波（.c/.h)
/*
* FileName : kalman_filter.h
* Date : 2022/3/31
*/
#ifndef _KALMAN_FILTER_H
#define _KALMAN_FILTER_H
/*
* NOTES: n表示状态为n维
* 测量总是为1维
*/
/* 1 维数据 */
typedef struct {
float x; /* 状态 */
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; /* 过程噪声协方差(预测) */
float r; /* 测量噪声协方差 */
float p; /* 估计误差协方差 */
float gain;
} kalman1_state;
/* 2 维数据 */
typedef struct {
float x[2]; /* 状态:[0]-角度[1]-角度差, 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]; /* 过程噪声协方差(预测),2x1 [q0,0; 0,q1] */
float r; /* 测量噪声协方差 */
float p[2][2]; /* 估计误差协方差,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*//*
* FileName : kalman_filter.c
* Date : 2022/3/31
*/
#include "kalman_filter.h"
/*
* @brief
* 初始化结构体kalman1_state
* 我在这个init函数中设置了一些默认值:
* a = 1;
* h = 1;
*
* NOTES: 请根据自己的要求更改 A,H,q,r
* @inputs
* state - 卡尔曼结构体
* init_x -初始x状态值
* init_p -初始估计误差协方差
* @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; /* 预测噪声协方差 */
state->r = 5e2;//10e-5; /* 测量误差协方差 */
}
/*
* @brief
* 1维卡尔曼滤波
* @inputs
* state - 卡尔曼结构体
* z_measure - 测量值
* @outputs
* @retval
* 估计结果
*/
float kalman1_filter(kalman1_state *state, float z_measure)
{
/* 预测 */
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 */
/* 测量 */
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
* 初始化结构体kalman1_state
* 我在这个init函数中设置了一些默认值:
* A = {{1, 0.1}, {0, 1}};
* H = {1,0};
*
* NOTES: 请根据自己的要求更改 A,H,q,r
*
* @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}}; /* 测量噪声协方差 */
state->q[0] = 10e-7;
state->q[1] = 10e-7;
state->r = 10e-7; /* 估计误差协方差 */
}
/*
* @brief
* 2维卡尔曼滤波
* @inputs
* state - 卡尔曼结构体
* z_measure - 测量值
* @outputs
* state->x[0] - 更新状态值，如角度、速度
* state->x[1] - 更新状态值，如差角，加速度
* state->p - 更新估计误差约定矩阵
* @retval
* 返回值等于state->x[0]，所以可能是角度或速度
*/
float kalman2_filter(kalman2_state *state, float z_measure)
{
float temp0 = 0.0f;
float temp1 = 0.0f;
float temp = 0.0f;
/* 预测 */
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];
/* 测量 */
/* gain = p * H^T * [r + H * p * H^T]^(-1), H^T 意思是转置. */
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);
/* 更新 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];
}
3.C++版本
关于输出角度和角加速度数值的滤波，也可以按自己的需求更改
/*
* FileName : kalman.cpp
* Date : 2022/3/31
*/
#include "Kalman.h"
Kalman::Kalman() {
/* 可以根据自己的情况设定下列值 */
Q_angle = 0.001f;
Q_bias = 0.003f;
R_measure = 0.03f;
angle = 0.0f; // 重置角度
bias = 0.0f; // 重置误差
P[0][0] = 0.0f; // 因为我们假设偏差为0，并且我们知道起始角度(使用setAngle)，所以误差协方差矩阵是这样设置的
P[0][1] = 0.0f;
P[1][0] = 0.0f;
P[1][1] = 0.0f;
};
// 角度单位： 度 角速度单位： 度/秒 时间单位： 秒
float Kalman::getAngle(float newAngle, float newRate, float dt) {
// 卡尔曼滤波模块
// 离散卡尔曼滤波器时间更新方程-时间更新
// 更新xhat - 提前预测状态
/* 第一步 */
rate = newRate - bias;
angle += dt * rate;
// 更新估计误差协方差-提前预测误差协方差
/* 第二步 */
P[0][0] += dt * (dt*P[1][1] - P[0][1] - P[1][0] + Q_angle);
P[0][1] -= dt * P[1][1];
P[1][0] -= dt * P[1][1];
P[1][1] += Q_bias * dt;
// 离散卡尔曼滤波器测量更新方程-测量更新
// 计算卡尔曼增益-估算卡尔曼增益
/* 第四步 */
float S = P[0][0] + R_measure;
/* 第五步 */
float K[2]; //卡尔曼增益-这是一个2x1的向量
K[0] = P[0][0] / S;
K[1] = P[1][0] / S;
// 计算角度和偏差-用测量zk更新估计(newAngle)
/* 第三步 */
float y = newAngle - angle;
/* 第六步 */
angle += K[0] * y;
bias += K[1] * y;
// 计算估计误差协方差-更新误差协方差
/* 第七步 */
float P00_temp = P[0][0];
float P01_temp = P[0][1];
P[0][0] -= K[0] * P00_temp;
P[0][1] -= K[0] * P01_temp;
P[1][0] -= K[1] * P00_temp;
P[1][1] -= K[1] * P01_temp;
return angle;
};
void Kalman::setAngle(float angle) { this->angle = angle; }; //用于设置角度时，应将此设置为起始角度
float Kalman::getRate() { return this->rate; };
/* 这些是用来调整卡尔曼滤波器的 */
void Kalman::setQangle(float Q_angle) { this->Q_angle = Q_angle; };
void Kalman::setQbias(float Q_bias) { this->Q_bias = Q_bias; };
void Kalman::setRmeasure(float R_measure) { this->R_measure = R_measure; };
float Kalman::getQangle() { return this->Q_angle; };
float Kalman::getQbias() { return this->Q_bias; };
float Kalman::getRmeasure() { return this->R_measure; };
/*
* FileName : kalman.h
* Date : 2022/3/31
*/
#ifndef _Kalman_h_
#define _Kalman_h_
class Kalman {
public:
Kalman();
// 角度的单位：度 速率的单位：度/秒 时间的单位：秒
float getAngle(float newAngle, float newRate, float dt);//主滤波函数 dt:采样频率
void setAngle(float angle); // 用于设置角度时，应将此设置为起始角度
float getRate(); // 返回无偏估计
/* 这些是用来调整卡尔曼滤波器的 */
void setQangle(float Q_angle);
/**
* setQbias(float Q_bias)
* 在Kalman.cpp的默认值是 (0.003f)
* 提高这个值来更紧密地跟踪输入，
* 降低这个值来平滑卡尔曼滤波的结果。
*/
void setQbias(float Q_bias);
void setRmeasure(float R_measure);
float getQangle();
float getQbias();
float getRmeasure();
private:
/*卡尔曼滤波器的变量 */
float Q_angle; // 处理加速度计的噪声方差
float Q_bias; // 陀螺仪偏差的处理噪声方差
float R_measure; // 测量噪声方差——这实际上是测量噪声的方差
float angle; // 由卡尔曼滤波器计算的角度- 2x1状态向量的一部分
float bias; // 由卡尔曼滤波器计算的陀螺偏置部分-2x1状态向量
float rate; // 无偏估计计算从速率和计算偏差-我们必须调用getAngle来更新速率
float P[2][2]; // 误差协方差矩阵-这是一个2x2矩阵
#endif
4.MPU6050滤波实例，亲测有效
#ifndef __KALMAN_H
#define __KALMAN_H
#include "math.h"
#define Pi 3.1415926
//噪声参数
#define dt 0.012 //kalman滤波器采样时间
#define R_angle 0.5
#define Q_angle 0.001 //角度数据置信度
#define Q_gyro 0.003 //角速度数据置信度
static float Angle_x_temp;
static float Angle_y_temp;
static float Angle_z_temp;
static float Angle_X_Final;
static float Angle_Y_Final;
static float Angle_Z_Final;
static short aacx, aacy, aacz; //加速度传感器原始数据
static short gyrox, gyroy, gyroz; //陀螺仪原始数据
double KalmanFilter(const double ResrcData, double ProcessNiose_Q, double MeasureNoise_R);
void Angle_Calculate(void);
void Kalman_Filter_X(float Accel,float Gyro);
void Kalman_Filter_Y(float Accel,float Gyro);
void Kalman_Filter_Z(float Accel, float Gyro);
#endif/***********************卡尔曼滤波算法***************************/
#include "Kalman.h"
float Accel_x;
float Accel_y;
float Accel_z;
float Gyro_x;
float Gyro_y;
float Gyro_z;
//其他地方已经定义
//float Angle_gy;
//static float Angle_x_temp;
//static float Angle_y_temp;
//static float Angle_z_temp;
//static float Angle_X_Final;
//static float Angle_Y_Final;
//static float Angle_Z_Final;
char C_0 = 1;
float Q_bias_x, Q_bias_y, Q_bias_z;
float Angle_err_x, Angle_err_y, Angle_err_z;
float PCt_0, PCt_1, E;
float K_0, K_1, t_0, t_1;
float Pdot[4] = { 0,0,0,0 };
float PP[2][2] = { { 1, 0 },{ 0, 1 } };
//卡尔曼理论算法
double KalmanFilter(const double ResrcData, double ProcessNiose_Q, double MeasureNoise_R)
{
double R = MeasureNoise_R; //测量噪声 R 增大，动态响应变慢，收敛稳定性变好
double Q = ProcessNiose_Q; //过程噪声 Q 增大，动态响应变快，收敛稳定性变坏
static double x_last;
double x_mid = x_last;
double x_now;
static double p_last;
double p_mid;
double p_now;
double kg; //卡尔曼增益
/*******预测过程*******/
x_mid = x_last; //x_last=x(k-1|k-1),x_mid=x(k|k-1)
p_mid = p_last + Q; //p_mid=p(k|k-1),p_last=p(k-1|k-1)
/*******更新过程*******/
kg = p_mid / (p_mid + R);
x_now = x_mid + kg*(ResrcData - x_mid);
p_now = (1 - kg)*p_mid;
p_last = p_now;
x_last = x_now;
return x_now;
}
//主调用函数
void Angle_Calculate(void)
{
//deg = rad*180/3.14
float x=0, y=0, z=0;
//初始变量赋值给计算变量
Accel_x = aacx;
Accel_y = aacy;
Accel_z = aacz;
Gyro_x = gyrox;
Gyro_y = gyroy;
Gyro_z = gyroz;
//根据实际给定的限制计算
//MPU6050初始化设置范围为2g时，灵敏度 16384 LSB/g
if (Accel_x<32764) x = Accel_x / 16384;
else x = 1 - (Accel_x - 49152) / 16384;
if (Accel_y<32764) y = Accel_y / 16384;
else y = 1 - (Accel_y - 49152) / 16384;
if (Accel_z<32764) z = Accel_z / 16384;
else z = (Accel_z - 49152) / 16384;
//计算与x轴的夹角
Angle_x_temp = (atan2(z , y)) * 180 / Pi;
Angle_y_temp = (atan2(x , z)) * 180 / Pi;
Angle_z_temp = (atan2(y , x)) * 180 / Pi;
if (Accel_y<32764) Angle_y_temp = +Angle_y_temp;
if (Accel_y>32764) Angle_y_temp = -Angle_y_temp;
if (Accel_x<32764) Angle_x_temp = +Angle_x_temp;
if (Accel_x>32764) Angle_x_temp = -Angle_x_temp;
if (Accel_z<32764) Angle_z_temp = +Angle_z_temp;
if (Accel_z>32764) Angle_z_temp = -Angle_z_temp;
if (Gyro_x<32768) Gyro_x = -(Gyro_x / 16.4); //去除零点偏移，计算角速度值,负号为方向处理
if (Gyro_x>32768) Gyro_x = +(65535 - Gyro_x) / 16.4;
if (Gyro_y<32768) Gyro_y = -(Gyro_y / 16.4);
if (Gyro_y>32768) Gyro_y = +(65535 - Gyro_y) / 16.4;
if (Gyro_z<32768) Gyro_z = -(Gyro_z / 16.4);
if (Gyro_z>32768) Gyro_z = +(65535 - Gyro_z) / 16.4;
Kalman_Filter_X(Angle_x_temp, Gyro_x); ;
Kalman_Filter_Y(Angle_y_temp, Gyro_y);
Kalman_Filter_Z(Angle_z_temp, Gyro_z);
}
void Kalman_Filter_X(float Accel, float Gyro)
{
/*****状态估计*****/
Angle_X_Final += (Gyro - Q_bias_x) * dt; //先验估计
Pdot[0] = Q_angle - PP[0][1] - PP[1][0];
Pdot[1] = -PP[1][1];
Pdot[2] = -PP[1][1];
Pdot[3] = Q_gyro;
/******方差估计*******/
PP[0][0] += Pdot[0] * dt;
PP[0][1] += Pdot[1] * dt;
PP[1][0] += Pdot[2] * dt;
PP[1][1] += Pdot[3] * dt;
/*****残差*****/
Angle_err_x = Accel - Angle_X_Final;
PCt_0 = C_0 * PP[0][0];
PCt_1 = C_0 * PP[1][0];
E = R_angle + C_0 * PCt_0;
/*****最优增益*****/
K_0 = PCt_0 / E;
K_1 = PCt_1 / E;
t_0 = PCt_0;
t_1 = C_0 * PP[0][1];
/*****数据更新*****/
PP[0][0] -= K_0 * t_0;
PP[0][1] -= K_0 * t_1;
PP[1][0] -= K_1 * t_0;
PP[1][1] -= K_1 * t_1;
Angle_X_Final += K_0 * Angle_err_x;
Q_bias_x += K_1 * Angle_err_x;
Gyro_x = Gyro - Q_bias_x;
}
void Kalman_Filter_Y(float Accel, float Gyro)
{
Angle_Y_Final += (Gyro - Q_bias_y) * dt;
Pdot[0] = Q_angle - PP[0][1] - PP[1][0];
Pdot[1] = -PP[1][1];
Pdot[2] = -PP[1][1];
Pdot[3] = Q_gyro;
PP[0][0] += Pdot[0] * dt;
PP[0][1] += Pdot[1] * dt;
PP[1][0] += Pdot[2] * dt;
PP[1][1] += Pdot[3] * dt;
Angle_err_y = Accel - Angle_Y_Final;
PCt_0 = C_0 * PP[0][0];
PCt_1 = C_0 * PP[1][0];
E = R_angle + C_0 * PCt_0;
K_0 = PCt_0 / E;
K_1 = PCt_1 / E;
t_0 = PCt_0;
t_1 = C_0 * PP[0][1];
PP[0][0] -= K_0 * t_0;
PP[0][1] -= K_0 * t_1;
PP[1][0] -= K_1 * t_0;
PP[1][1] -= K_1 * t_1;
Angle_Y_Final += K_0 * Angle_err_y;
Q_bias_y += K_1 * Angle_err_y;
Gyro_y = Gyro - Q_bias_y;
}
void Kalman_Filter_Z(float Accel, float Gyro)
{
Angle_Z_Final += (Gyro - Q_bias_z) * dt;
Pdot[0] = Q_angle - PP[0][1] - PP[1][0];
Pdot[1] = -PP[1][1];
Pdot[2] = -PP[1][1];
Pdot[3] = Q_gyro;
PP[0][0] += Pdot[0] * dt;
PP[0][1] += Pdot[1] * dt;
PP[1][0] += Pdot[2] * dt;
PP[1][1] += Pdot[3] * dt;
Angle_err_z = Accel - Angle_Z_Final;
PCt_0 = C_0 * PP[0][0];
PCt_1 = C_0 * PP[1][0];
E = R_angle + C_0 * PCt_0;
K_0 = PCt_0 / E;
K_1 = PCt_1 / E;
t_0 = PCt_0;
t_1 = C_0 * PP[0][1];
PP[0][0] -= K_0 * t_0;
PP[0][1] -= K_0 * t_1;
PP[1][0] -= K_1 * t_0;
PP[1][1] -= K_1 * t_1;
Angle_Z_Final += K_0 * Angle_err_z;
Q_bias_z += K_1 * Angle_err_z;
Gyro_z = Gyro - Q_bias_z;
}
相关文章
- 直接在代码里面对list集合进行分页
- .NET Framework 4.5新特性详解
- 大数据的简要介绍
- 大数据的由来
- 高斯混合模型的自然梯度变量推理
- timing-wheel 仿Kafka实现的时间轮算法
- 使用Navicat软件连接自建数据库（Linux系统）
- 那一天，我被Redis主从架构支配的恐惧
- Redis 深入了解键的过期时间
- C#使用委托调用实现用户端等待闪屏
- 基于流计算 Oceanus 和 Elasticsearch Service 构建百亿级实时监控系统
- GRAND | 转录调控网络预测数据库
- JFreeChart API中文文档
- 临床相关突变查询数据库
- TIGER | 人类胰岛基因变化查询数据库
- 视频边缘计算网关EasyNVR在视频整体监控解决方案中的应用分析
- Apache Arrow - 大数据在数据湖后的下一个风向标
- 常见的电商数据指标体系
- AKShare-艺人数据-艺人流量价值
- MySQL中多表联合查询与子查询的这些区别，你可能不知道！