-
Notifications
You must be signed in to change notification settings - Fork 7
/
Kalman.c
147 lines (88 loc) · 4.67 KB
/
Kalman.c
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
#include "common.h" //包含系统配置
/******************************************
#include "system_MKL.h" //系统配置
#include "PORT_cfg.h" //管脚复用配置
#include "MKL_mcg.h" //K60 时钟模块
*******************************************/
#include "include.h"//包含用户定义的头文件
#define SysNoiseCoVar (0.0123)
#define MeasNoiseCoVar (16.7415926) //系统噪声协方差，测量噪声协方差
//SysNoiseCoVar越大，精度越低，速度越快MeasNoiseCovar越大，精度越高，速度越慢
volatile u32 AdjustVar1=0;//调整值
volatile u32 AdjustVar2=0;
volatile u32 AdjustVar3=0;
u8 MeasMatrix[3]; //测量值矩阵
/*卡尔曼滤波需要设置4个值，其余均可自动调整*/
u8 KalmanFitterAD1(u8 MeasVar)
{
static double PreOptimalVar=1; //先前最优值
static double CurMeasVar=0,CurEstimateVar=0; //当前测量值，当前预测值
static double CurOptimalVar=0; //当前最优值
static double CurSysCoVar=0,PreSysCoVar=30; //当前系统协方差，先前系统协方差
static double KalmanGain=0; //卡尔曼增益
CurMeasVar=MeasVar; //当前系统测量值
//先前系统最优值和先前系统协方差需要设置非零初始值；；；；
/*当前估计值=先前最优值+调整值*/
CurEstimateVar=PreOptimalVar+AdjustVar1;
AdjustVar1=0; //设定调整值的原因是加快调整速度
/*当前系统协方差=先前系统协方差+系统噪声协方差*/
CurSysCoVar=PreSysCoVar+SysNoiseCoVar;
/*卡尔曼增益=当前系统协方差/(当前系统协方差+测量噪声协方差)*/
KalmanGain=CurSysCoVar/(CurSysCoVar+MeasNoiseCoVar);
/*当前系统最优值=当前系统估计值+卡尔曼增益*（测量值-当前系统估计值）*/
CurOptimalVar=CurEstimateVar+KalmanGain*(CurMeasVar-CurEstimateVar);
/*先前系统噪声协方差=（1-卡尔曼增益）x当前系统协方差*/
PreSysCoVar =(1-KalmanGain)*CurSysCoVar;
PreOptimalVar=CurOptimalVar; //递归处理当前系统最优值
/*返回数据*/
return (u8)CurOptimalVar;
}
u8 KalmanFitterAD2(u8 MeasVar)
{
static double PreOptimalVar=1; //先前最优值
static double CurMeasVar=0,CurEstimateVar=0; //当前测量值，当前预测值
static double CurOptimalVar=0; //当前最优值
static double CurSysCoVar=0,PreSysCoVar=30; //当前系统协方差，先前系统协方差
static double KalmanGain=0; //卡尔曼增益
CurMeasVar=MeasVar; //当前系统测量值
//先前系统最优值和先前系统协方差需要设置非零初始值；；；；
/*当前估计值=先前最优值+调整值*/
CurEstimateVar=PreOptimalVar+AdjustVar2;
AdjustVar2=0; //设定调整值的原因是加快调整
/*当前系统协方差=先前系统协方差+系统噪声协方差*/
CurSysCoVar=PreSysCoVar+SysNoiseCoVar;
/*卡尔曼增益=当前系统协方差/(当前系统协方差+测量噪声协方差)*/
KalmanGain=CurSysCoVar/(CurSysCoVar+MeasNoiseCoVar);
/*当前系统最优值=当前系统估计值+卡尔曼增益*（测量值-当前系统估计值）*/
CurOptimalVar=CurEstimateVar+KalmanGain*(CurMeasVar-CurEstimateVar);
/*先前系统噪声协方差=（1-卡尔曼增益）x当前系统协方差*/
PreSysCoVar =(1-KalmanGain)*CurSysCoVar;
/*处理递归的任务*/
PreOptimalVar=CurOptimalVar;
/*返回数据*/
return (u8)CurOptimalVar;
}
u8 KalmanFitterAD3(u8 MeasVar)
{
static double PreOptimalVar=1; //先前最优值
static double CurMeasVar=0,CurEstimateVar=0; //当前测量值，当前预测值
static double CurOptimalVar=0; //当前最优值
static double CurSysCoVar=0,PreSysCoVar=30; //当前系统协方差，先前系统协方差
static double KalmanGain=0; //卡尔曼增益
CurMeasVar=MeasVar; //当前系统测量值
//先前系统最优值和先前系统协方差需要设置非零初始值；；；；
/*当前估计值=先前最优值+调整值*/
CurEstimateVar=PreOptimalVar+AdjustVar3;
AdjustVar3=0; //设定调整值的原因是加快调整
/*当前系统协方差=先前系统协方差+系统噪声协方差*/
CurSysCoVar=PreSysCoVar+SysNoiseCoVar;
/*卡尔曼增益=当前系统协方差/(当前系统协方差+测量噪声协方差)*/
KalmanGain=CurSysCoVar/(CurSysCoVar+MeasNoiseCoVar);
/*当前系统最优值=当前系统估计值+卡尔曼增益*（测量值-当前系统估计值）*/
CurOptimalVar=CurEstimateVar+KalmanGain*(CurMeasVar-CurEstimateVar);
/*先前系统噪声协方差=（1-卡尔曼增益）x当前系统协方差*/
PreSysCoVar =(1-KalmanGain)*CurSysCoVar;
PreOptimalVar=CurOptimalVar;
/*返回数据*/
return (u8)CurOptimalVar;
}