You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
20 lines
1.0 KiB
20 lines
1.0 KiB
#pragma once
|
|
/***********************************************************************************************************************
|
|
* ====================================================卡尔曼滤波===================================================== *
|
|
***********************************************************************************************************************/
|
|
typedef struct {
|
|
float LastP; //上次估算协方差 初始化值为0.02
|
|
float Now_P; //当前估算协方差 初始化值为0
|
|
float out; //卡尔曼滤波器输出 初始化值为0
|
|
float Kg; //卡尔曼增益 初始化值为0
|
|
float Q; //过程噪声协方差 初始化值为0.001
|
|
float R; //观测噪声协方差 初始化值为0.543
|
|
} KFP; // Kalman Filter parameter
|
|
|
|
/**
|
|
*卡尔曼滤波器
|
|
*@param KFP *kfp 卡尔曼结构体参数
|
|
* float input 需要滤波的参数的测量值（即传感器的采集值）
|
|
*@return 滤波后的参数（最优值）
|
|
*/
|
|
float kalmanFilter(KFP* kfp, float input);
|