|
卡尔曼滤波算法,自己,输入为随机数,Q和R要自己选
主函数:
#include "kalman.h"
#include "stdio.h"
#include "stdlib.h"
void main(void)
{
KalmanCountData k;
//定义一个卡尔曼运算结构体
Kalman_Filter_Init(&k);
//讲运算变量初始化
int n;
int a;
srand((unsigned)time(NULL)); // 初始化随机种子,使rand()产生的随机数每次不一样
for(a = 0;a<1000;a++)
//测试1000次
{
//m,n为1到100的随机数
n = 1+ rand() %100;
//卡尔曼滤波,传递2个测量值以及运算结构体
Kalman_Filter(&k,n);
//打印结果
printf("第%d次迭代\n 输入:%d, 输出滤波值为:%f\r\n",a+1,n,k.Filter_Value);
}
}
kalman.h:
typedef struct //定义结构体
{
float Filter_Value; //K-1时刻的系滤波值
float A; //系统参数,一维模型中通常选为1
float H; //系统测量参数,一维模型选择为1
float Kg; //卡尔曼增益
float Q; //过程噪声偏差
float R; //测量噪声偏差
float P; //估计误差协方差
} KalmanCountData;
void Kalman_Filter_Init(KalmanCountData * Kalman_Struct) //卡尔曼滤波器初始化
{
Kalman_Struct->A = 1;
Kalman_Struct->H = 1;
Kalman_Struct->P = 3; //后验状态估计值方差的初始值
Kalman_Struct->Q = 0.1; //过程噪声偏差,实验测的 ,要填的!
Kalman_Struct->R = 10; //测量噪声偏差,要填的!
Kalman_Struct->Filter_Value = 0; //初始化设置k-1时刻的系统滤波后的值为0
}
double Kalman_Filter(KalmanCountData* Kalman_Struct, double Last_Measurement){
double PredictValue = Kalman_Struct->Filter_Value*Kalman_Struct->A; //先验估计的预测值
Kalman_Struct->P = Kalman_Struct->A*Kalman_Struct->P*Kalman_Struct->A+Kalman_Struct->Q; //求协方差
double prevalve = Kalman_Struct->Filter_Value; //记录上一次(k-1)的值
Kalman_Struct->Kg = Kalman_Struct->P*Kalman_Struct->H/(Kalman_Struct->H*Kalman_Struct->P*Kalman_Struct->H + Kalman_Struct->R);
/*
卡尔曼增益Kg = P(k|k-1) H’ / (H P(k|k-1) H’ + R)
*/
Kalman_Struct->Filter_Value = PredictValue + (Last_Measurement-PredictValue) * Kalman_Struct->Kg;
/*
修正结果,更新预测值;X(k|k) = X(k|k-1) + (Z(k) - H* X(k|k-1)) * Kg
*/
Kalman_Struct->P = (1 - Kalman_Struct->Kg * Kalman_Struct->H) * Kalman_Struct->P;
//更新后验估计的协方差,P(k|k) = (1 - Kg * H) * P(k|k-1)
return Kalman_Struct->Filter_Value;
}
|
评分
-
查看全部评分
|