卡尔曼角度滤波

#define _CRT_SECURE_NO_WARNINGS
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <math.h>
#include <time.h>



typedef struct KALMAN {
	double Q;
	double R;
	float old_value;
	float scope;
	float accumulated_error;
	int kal_mode;
}Kalman_t;

//sqrtf(x,y)  平方根 单精度
//fabsf(x +/- y)  取绝对值 输出为 正 单精度数
//powf(x,y) x的y次方 输出类型为单精度


//卡尔曼滤波变量
Kalman_t kalman;


float yaw_signed(float yaw) //入参yaw为 0-360  返回 +-180   左正 右负  
{
	if (yaw > 180) {
		yaw = yaw - 360;	//将360度 转成 +-180度 方法(目标角)
	}
	yaw = -yaw;				// 与板安装方式有关
	return yaw;
}

/*
* 卡尔曼初始化 yaw_mode 改为布尔模式
* scope:	初始值
* yaw_mode:	模式 1为角度滤波模式 
* kal :		结构体参数
*/
void kalman_init(float scope, int mode, Kalman_t* kal) {
	float old_value = scope;

	if (mode) {
		old_value = scope + 360;
	}

	kal->Q = 0.00190;          //上轮预估误差
	kal->R = 0.04280;          //本轮预估误差
	kal->accumulated_error = 0;
	kal->old_value = old_value;
	kal->scope = old_value;
	kal->kal_mode = mode;
}

/*
* 卡尔曼滤波
* data:		需要滤波的数据
* kal :		结构体参数
*/
float kalman_filter(float data, Kalman_t* kal)
{
	float old_input = 0;     //上一轮的的输入
	float old_error_all = 0; //上一轮的总误差
	float trust_degree = 0;  //相信度
	float kalman_vlaue = 0;  //卡尔曼滤波值
	float new_vlaue = 0;
	float difference_value = 0;

	if (kal->kal_mode) {
		new_vlaue = (data + 360.0);
		difference_value = new_vlaue - kal->scope;
		if (difference_value > 340) {//差值 350度  允许误差 20度
			new_vlaue -= 360;
		}else if (difference_value < -340) {
			new_vlaue += 360;
		}
	}
	else {
		new_vlaue = data;
	}

	//新值与旧的值太大时进行跟踪
	if ((fabsf(new_vlaue - kal->old_value) / kal->scope) > 0.25) {
		//更新任测量值
		old_input = new_vlaue * 0.2 + kal->old_value * 0.8;
	}
	else {
		old_input = kal->old_value;
	}

	//上一轮的 总误差^2 = 累计误差^2 + 预估误差^2  (Q为上一轮的心理误差)
	old_error_all = sqrtf((powf(kal->accumulated_error, 2) + powf(kal->Q, 2)));
	//利用方差计算出来双方的信任度，R为这一轮的预估误差  总误差的^2 / (总误差^2 + 本轮预估误差^2)
	trust_degree = powf(old_error_all, 2) / (powf(old_error_all, 2) + powf(kal->R, 2));
	//滤波值 =（测量值-旧值）* 信任度
	kalman_vlaue = old_input + trust_degree * (new_vlaue - old_input);
	//计算累积误差 累积误差^2 = (1 - 想信度) * 上一轮总误差^2
	kal->accumulated_error = sqrtf((1 - trust_degree) * powf(old_error_all, 2));
	//新值保存为旧值
	kal->old_value = kalman_vlaue;
	//保存旧值范围
	kal->scope = new_vlaue;

	if (kal->kal_mode) {
		return fmod(kalman_vlaue, 360.0);
	}
	else {
		return kalman_vlaue;
	}
}


//float kalman_filter(float measure_value)
//{
//	float old_input = 0;     //上一轮的的输入
//	float old_error_all = 0; //上一轮的总误差
//	float trust_degree = 0;  //相信度
//	float kalman_vlaue = 0;  //卡尔曼滤波值
//	float new_vlaue = (measure_value + 360.0);
//	float difference_value = new_vlaue - kalman.scope;
//
//	if (difference_value > 340) //差值 350度
//	{
//		new_vlaue -= 360;
//	}
//	else if (difference_value < -340) {
//		new_vlaue += 360;
//	}
//
//	//新值与旧的值太大时进行跟踪
//	if ((fabsf(new_vlaue - kalman.old_value) / kalman.scope) > 0.25)
//	{
//		//更新任测量值
//		old_input = new_vlaue * 0.2 + kalman.old_value * 0.8;
//	}
//	else
//	{
//		old_input = kalman.old_value;
//	}
//	//上一轮的 总误差^2 = 累计误差^2 + 预估误差^2  (Q为上一轮的心理误差)
//	old_error_all = sqrtf((powf(kalman.accumulated_error, 2) + powf(kalman.Q, 2)));
//	//利用方差计算出来双方的信任度，R为这一轮的预估误差  总误差的^2 / (总误差^2 + 本轮预估误差^2)
//	trust_degree = powf(old_error_all, 2) / (powf(old_error_all, 2) + powf(kalman.R, 2));
//	//滤波值 =（测量值-旧值）* 信任度
//	kalman_vlaue = old_input + trust_degree * (new_vlaue - old_input);
//	//计算累积误差 累积误差^2 = (1 - 想信度) * 上一轮总误差^2
//	kalman.accumulated_error = sqrtf((1 - trust_degree) * powf(old_error_all, 2));
//	//新值保存为旧值
//	kalman.old_value = kalman_vlaue;
//	//保存旧值范围
//	kalman.scope = new_vlaue;
//	return fmod(kalman_vlaue, 360.0);
//}

#define LEN_MAX 1000
float Data[LEN_MAX] = { 0.0 };
float KalMan_Value[LEN_MAX] = { 0.0 };
//#define YAW (0) //当前角度

void random_value(float yaw)
{
	srand((unsigned)time(NULL));
	for (int i = 0; i < LEN_MAX; i++) {
		Data[i] = fmod(((yaw - 10) + ((rand() % 200) / 10.0) + 360.0), 360.0);   // +- 10误差
		/*printf("arr[%d]:%f \n", i, Data[i]);*/
	}
}

int main(void)
{
	printf("\n=====================================================\n");
	kalman_init(300, 0, &kalman);
	random_value(300);//随机种子 初始值 

	for (int i = 0; i < LEN_MAX; i++) {
		KalMan_Value[i] = kalman_filter(Data[i],&kalman);
	}

	for (int i = 0; i < LEN_MAX; i++) {
		printf("arr[%d]:%04.2f --> kam[%d]:%04.2f\n", i, Data[i], i, KalMan_Value[i]);
	}
	printf("\n=====================================================\n");

	while (1)
	{
		;
	}

	return 0;
}