最优控制，卡尔曼滤波器 ,算法实现步骤：
- 获取当前时刻的仪器"测量值" 。
- 获取上一时刻的 "预测量值" 和 "误差",
- 计算得到当前的最优量值。
- 再预测下一刻的测量值。
公式：
首先，我们先要引入一个离散控制过程的系统。该系统可用一个线性随机微分方程来描述：
再加上系统的测量值：
公式中：（对于单测量系统，，a、b、h为参数。如果对于多测量系统，a、b、h为矩阵参数）
x(k)是k时刻的系统状态。
u(k)是k时刻对系统的控制量。
a和b是系统参数。
z(k)是k时刻的测量值。
h是测量系统的参数。
w(k)和v(k)分别表示过程和测量的噪声。他们被假设成高斯白噪声(White Gaussian Noise)，他们的协方差(covariance)分别是Q，R（这里我们假设他们不随系统状态变化而变化）
涉及很多理论知识的地方，会有些吃力，我们关注结果就行，“卡尔曼滤波器”现在应用非常成熟，我们会用就行。
卡尔曼滤波算法*.h文件
/*
卡尔曼滤波，是根据估计值(此处的估计值一般默认为上次的最优值)和测量值，计算一个最优值做为结果，这个最优值即为滤波后的值
使用说明:
使用需要初始化卡尔曼滤波参数，调用一次vKalmanFilterInit()函数即可。
之后每次测得的值传递到_KalmanFilter()函数，返回的即是滤波后的最优值。
*/
====================Kalman.h文件===================
#ifndef _KALMAN_H
#define _KALMAN_H
#include "math.h"
#define DE_KalmanResult_TYPE double
typedef struct {
DE_KalmanResult_TYPE _KalmanGain;//卡尔曼增益
DE_KalmanResult_TYPE _EstimateCovariance;//估计协方差
DE_KalmanResult_TYPE _MeasureCovariance;//测量协方差
DE_KalmanResult_TYPE _OptimumValue;//最优值
}KalmanType;
extern KalmanType stKalmanParameter;
void vKalmanFilterInit(DE_KalmanResult_TYPE _EstimateCovariance1,DE_KalmanResult_TYPE _MeasureCovariance1,DE_KalmanResult_TYPE _OptimumValue1);//卡尔曼滤波初始化
DE_KalmanResult_TYPE _KalmanFilter(DE_KalmanResult_TYPE _MeasureValue);//卡尔曼滤波计算
#endif卡尔曼滤波算法*.c文件
#include "Kalman.h"
KalmanType stKalmanParameter;
void vKalmanFilterInit(DE_KalmanResult_TYPE _EstimateCovariance1,DE_KalmanResult_TYPE _MeasureCovariance1,DE_KalmanResult_TYPE _OptimumValue1){//卡尔曼滤波初始化
stKalmanParameter._EstimateCovariance = _EstimateCovariance1;//0.1;
stKalmanParameter._MeasureCovariance = _MeasureCovariance1;//0.02;
stKalmanParameter._OptimumValue = _OptimumValue1;//0;
}
DE_KalmanResult_TYPE _KalmanFilter(DE_KalmanResult_TYPE _MeasureValue){//卡尔曼滤波计算
//计算卡尔曼增益
stKalmanParameter._KalmanGain = stKalmanParameter._EstimateCovariance * \
sqrt(1/(stKalmanParameter._EstimateCovariance * stKalmanParameter._EstimateCovariance + \
stKalmanParameter._MeasureCovariance * stKalmanParameter._MeasureCovariance));
//计算本次滤波最优值
stKalmanParameter._OptimumValue = stKalmanParameter._OptimumValue + \
stKalmanParameter._KalmanGain*(_MeasureValue - stKalmanParameter._OptimumValue);
//更新估计协方差
stKalmanParameter._EstimateCovariance = sqrt(1 - stKalmanParameter._KalmanGain) * \
stKalmanParameter._EstimateCovariance;
//更新测量协方差
stKalmanParameter._MeasureCovariance = sqrt(1 - stKalmanParameter._KalmanGain) * \
stKalmanParameter._MeasureCovariance;
//返回最优值
return stKalmanParameter._OptimumValue;
}demo文件
/*以下是demo例程数组a[]是1~10之间的随机数，通过对这些随机数做卡尔曼滤波，最后得出的结果见下图。*/
=============KalmanDemo.c文件=============
#include <stdio.h>
#include <stdlib.h>
#include "math.h"
#define DE_KalmanResult_TYPE double
/*
int a[]={
11,12,15,18,20,19,16,19,16,20,13,20,20,15,14,15,12,20,15,12,11,18,15,19,15,12,14,19,12,15,15,13,13,12,16,17,20,16,19,20,19,11,18,16,11,11,11,19,20,12,17,18,15,12,16,15,20,16,18,18,15,16,18,20,13,11,11,14,17,20,11,19,14,16,17,19,16,13,11,18,14,15,16,15,11,19,20,16,19,18,11,12,18,18,20,16,16,19,12,13,18,19,19,12,14,18,18,11,11,20,19,20,15,14,11,20,20,19,19,15,15,16,20,11,19,18,11,14,20,15,19,17,12,12,18,17,11,20,13,17,14,13,14,15,14,15,13,18,14,14,18,14,13,15,14,15,16,18,16,18,19,13,13,19,12,19,13,13,11,15,19,18,15,20,16,11,14,11,16,13,15,19,19,18,19,17,16,13,15,12,19,18,14,15,15,16,18,15,14,15,20,11,17,18,15,14,14,19,15,13,20,15,15,20,13,18,18,16,15,12,15,19,13,11,15,18,19,13,15,15,11,13,15,17,18,20,17,19,17,17,20,13,16,15,15,11,18,20,15,15,17,12,12,19,15,12,20,15,11,19,12,12,16,16,13,16,11,16,13,16,18,16,18,18,17,15,11,12,19,12,19,11,16,15,12,16,20,16,18,12,18,11,13,16,15,11,14,19,14,13,18,14,19,17,19,19,15,12,20,20,18,13,20,17,16,19,13,12,16,19,16,18,19,12,19,19,18,14,20,13,17,16,14,14,18,16,14,20,19,17,18,
};
*/
int a[]={
1,2,5,8,10,9,6,9,6,10,3,10,10,5,4,5,2,10,5,2,1,8,5,9,5,2,4,9,2,5,5,3,3,2,6,7,10,6,9,10,9,1,8,6,1,1,1,9,10,2,7,8,5,2,6,5,10,6,8,8,5,6,8,10,3,1,1,4,7,10,1,9,4,6,7,9,6,3,1,8,4,5,6,5,1,9,10,6,9,8,1,2,8,8,10,6,6,9,2,3,8,9,9,2,4,8,8,1,1,10,9,10,5,4,1,10,10,9,9,5,5,6,10,1,9,8,1,4,10,5,9,7,2,2,8,7,1,10,3,7,4,3,4,5,4,5,3,8,4,4,8,4,3,5,4,5,6,8,6,8,9,3,3,9,2,9,3,3,1,5,9,8,5,10,6,1,4,1,6,3,5,9,9,8,9,7,6,3,5,2,9,8,4,5,5,6,8,5,4,5,10,1,7,8,5,4,4,9,5,3,10,5,5,10,3,8,8,6,5,2,5,9,3,1,5,8,9,3,5,5,1,3,5,7,8,10,7,9,7,7,10,3,6,5,5,1,8,10,5,5,7,2,2,9,5,2,10,5,1,9,2,2,6,6,3,6,1,6,3,6,8,6,8,8,7,5,1,2,9,2,9,1,6,5,2,6,10,6,8,2,8,1,3,6,5,1,4,9,4,3,8,4,9,7,9,9,5,2,10,10,8,3,10,7,6,9,3,2,6,9,6,8,9,2,9,9,8,4,10,3,7,6,4,4,8,6,4,10,9,7,8,4,4,9,5,4,2,3,7,3,4,9,8,3,7,2,1,2,10,6,8,2,9,3,1,5,3,7,10,6,9,3,4,8,3,1,8,8,7,8,6,6,8,5,8,7,2,1,10,7,10,6,1,8,6,7,10,6,1,6,6,4,7,6,7,1,10,4,8,8,1,1,3,2,1,7,1,9,7,2,6,10,9,7,1,10,6,8,9,3,9,4,5,5,8,1,10,7,1,6,3,4,5,3,8,1,8,8,10,5,6,5,5,1,6,2,10,4,8,6,2,10,9,10,2,3,8,7,4,3,3,9,5,3,2,2,9,5,8,7,9,4,8,4,6,7,3,6,6,10,1,7,4,4,5,3,2,4,4,6,3,3,8,3,5,7,2,10,9,7,6,6,9,7,8,5,8,3,10,8,4,4,3,10,8,9,5,10,9,8,2,9,10,10,4,1,10,7,3,6,10,8,9,5,3,8,1,10,1,1,9,9,3,9,7,1,2,6,3,1,3,3,2,7,7,1,9,7,3,9,5,4,5,4,2,10,2,5,1,7,3,3,9,9,10,8,2,2,6,9,5,10,6,6,9,6,8,4,7,6,6,5,7,2,5,10,6,2,9,4,8,7,3,5,4,8,8,8,4,7,5,2,7,3,2,9,5,4,5,2,9,4,3,5,7,8,6,1,3,4,8,9,6,9,4,1,10,10,1,4,10,7,5,3,3,3,8,10,6,6,2,2,4,2,4,10,8,2,3,7,5,3,4,2,4,4,7,9,6,4,5,10,2,5,3,9,9,5,2,5,3,5,9,9,5,4,6,8,7,5,8,4,6,5,10,4,5,7,4,2,7,3,6,9,2,10,7,10,8,7,9,2,8,5,7,4,3,4,2,7,8,8,7,9,3,1,5,2,4,5,8,6,5,3,2,10,6,4,5,9,2,3,10,2,2,8,10,10,2,1,6,9,8,8,3,7,6,4,2,7,5,2,6,3,8,1,10,3,7,3,1,2,9,4,8,7,5,2,1,7,5,7,3,1,4,8,10,7,7,5,7,5,7,3,3,9,3,9,2,1,7,3,7,2,9,1,9,2,8,4,4,3,1,6,6,5,7,2,7,4,5,7,3,9,6,3,3,1,3,5,6,2,6,3,7,4,6,7,2,10,2,4,1,7,10,2,1,1,2,9,2,6,2,3,10,6,2,4,7,2,5,2,5,4,3,5,9,10,10,2,4,4,10,1,7,1,2,9,3,9,1,5,10,3,8,9,6,2,10,9,8,1,3,1,6,10,4,7,7,9,8,7,9,1,9,7,2,6,2,3,10,4,10,6,6,5,3,1,6,10,9,4,8,1,5,10,2,4,10,10,2,7,2,10,2,3,6,4,3,8,1,7,2,3,10,10,2,8,5,3,10,5,4,7,5,5,5,9,5,8,5,6,2,7,8,1,10,8,1,5,6,8,7,10,1,6,10,7,5,6,3,8,7,1,9,10,9,9,10,4,8,3,10,2,2,1,1,7,8,10,7,2,4,8,2,9,8,10,4,8,3,9,10,2,6,3,7,7,3,3,5,7,3,8,10,2,8,5,2,7,1,10,9,3,2,9,2,2,1,7,10,5,9,2,9,10,5,4,8,10,4,7,8,2,3,3,4,2,10,1,9,1,10,10,4,5,8,6,6,6,8,10,3,4,10,1,3,7,3,1,2,6,3,6,8,10,3,9,1,6,1,7,8,4,10,1,5,9,8,5,10,9,7,2,10,6,3,5,6,3,5,2,2,1,2,8,1,10,10,10,6,6,7,9,9,7,1,10,5,7,1,10,3,8,9,3,6,10,3,6,2,9,9,1,10,2,7,6,1,3,7,7,2,1,6,8,6,6,7,7,8,10,10,5,2,4,3,8,9,1,1,2,4,4,8,1,3,6,5,2,7,5,4,1,7,2,7,6,4,6,5,10,8,1,2,3,4,6,1,3,3,4,3,9,5,5,5,4,10,7,10,5,7,6,6,8,9,6,5,6,7,6,10,4,3,9,8,7,9,10,7,10,8,6,8,8,4,7,9,7,3,6,7,10,9,6,4,1,10,9,4,5,2,3,1,4,3,2,7,6,6,10,1,4,8,2,9,9,6,10,4,8,6,7,4,5,4,8,6,2,7,7,2,5,5,7,7,7,7,7,7,3,2,2,4,8,8,1,3,4,6,4,9,5,1,5,1,3,4,1,2,5,7,1,7,3,10,1,5,8,4,9,10,2,10,5,2,7,2,10,5,8,5,6,6,3,4,7,3,5,4,9,3,1,7,5,6,3,1,4,3,6,5,10,9,5,4,2,2,5,3,5,1,5,9,5,10,5,7,8,3,2,5,6,7,9,3,7,3,4,2,6,7,2,3,10,7,8,2,4,9,2,1,5,6,2,8,9,1,3,9,1,1,8,7,10,7,8,3,5,6,9,4,6,2,8,5,4,1,7,9,7,6,8,7,5,1,1,10,7,9,3,5,2,9,3,2,3,8,2,1,5,8,1,8,3,10,5,7,7,9,3,10,3,6,9,10,7,8,6,1,4,5,5,1,10,4,8,9,8,2,3,10,10,6,2,3,4,8,3,5,3,1,8,6,6,8,9,3,8,2,6,5,7,4,9,4,4,7,3,5,8,4,7,4,5,1,8,10,3,5,10,6,10,4,10,6,10,3,2,7,10,6,7,6,1,2,4,8,5,6,3,7,3,10,5,10,4,10,3,8,4,3,5,8,7,3,5,2,2,9,5,5,3,8,7,5,4,10,1,7,2,1,7,2,3,1,9,7,8,10,1,4,2,4,3,5,6,1,6,3,4,5,2,5,7,6,6,9,10,8,7,3,9,7,8,3,3,10,8,4,8,2,7,2,1,1,5,9,9,9,9,2,7,4,8,8,4,8,1,8,2,9,10,7,2,10,7,1,10,10,2,7,10,2,5,10,9,8,8,4,9,4,5,9,10,2,10,7,3,6,9,1,5,8,3,1,9,5,3,9,6,1,6,6,10,8,5,5,6,6,4,2,1,4,5,9,6,6,7,3,4,10,7,3,5,3,1,3,10,8,6,8,8,7,10,3,9,5,10,7,1,7,9,2,1,1,5,1,1,4,1,1,4,1,4,10,1,5,8,5,6,6,3,7,2,7,8,7,5,9,8,7,8,3,8,3,7,10,10,8,5,7,7,1,8,5,9,9,6,5,7,5,9,1,7,6,2,1,7,6,6,10,3,5,8,1,3,6,5,5,9,3,3,7,4,10,2,4,8,1,5,4,2,6,3,5,9,5,9,1,2,8,6,5,8,7,7,3,9,6,8,3,10,10,3,2,3,6,8,4,6,6,2,3,9,2,7,10,7,5,7,2,10,1,10,9,2,10,5,1,10,2,10,5,10,9,2,5,4,1,4,2,4,7,5,3,3,9,7,10,7,8,10,5,6,10,4,4,9,7,4,6,4,8,5,4,7,10,7,3,7,8,2,5,10,3,1,10,4,7,10,4,7,1,4,5,10,5,1,7,10,1,2,3,9,7,9,6,8,10,1,5,1,5,2,5,9,7,7,7,9,3,7,10,3,5,6,3,9,5,10,10,1,7,2,4,7,9,6,8,4,4,8,1,5,8,8,7,9,3,4,9,10,3,10,1,5,5,3,10,6,3,9,8,4,2,1,10,7,3,3,3,2,2,9,10,10,6,8,10,8,6,6,6,8,8,7,6,10,1,7,1,6,1,10,5,1,5,7,6,6,8,2,2,1,7,6,4,4,3,9,8,2,9,2,6,4,1,2,5,6,8,4,1,9,10,5,7,4,5,4,10,8,6,1,6,2,5,8,1,7,8,8,10,4,10,9,8,3,8,6,9,5,7,1,5,5,2,4,9,3,5,6,9,7,9,6,8,8,9,7,6,1,10,6,2,1,8,1,4,10,3,6,2,7,5,6,2,3,9,5,6,6,4,8,1,1,1,5,9,1,10,3,6,3,7,3,9,10,4,5,4,10,4,7,4,4,6,5,3,6,9,7,8,10,4,7,4,3,4,7,9,5,6,5,7,2,9,6,7,9,6,7,9,1,4,9,3,8,8,7,7,7,7,9,8,10,2,10,1,1,4,1,7,1,5,10,10,8,3,7,5,9,3,9,6,8,1,10,2,5,3,2,10,1,8,10,6,2,10,8,4,3,2,2,5,9,1,7,6,7,3,4,6,8,6,1,9,4,6,4,7,2,3,2,5,5,3,8,5,8,9,1,4,3,4,10,7,2,4,2,9,4,8,10,2,3,3,7,9,5,9,10,10,1,1,8,10,4,2,7,1,7,3,2,4,1,1,3,9,3,4,9,5,5,2,2,10,6,4,2,7,5,10,5,2,1,3,5,7,1,1,7,7,4,2,6,6,4,8,5,3,2,3,9,8,7,4,7,7,2,8,4,8,1,4,7,7,2,5,4,2,9,8,8,6,6,10,4,3,4,7,5,3,8,1,7,7,7,8,10,4,5,6,10,2,6,2,8,4,2,4,3,8,4,10,6,1,8,1,3,3,2,4,2,6,8,5,5,2,6,9,5,7,5,2,3,10,3,3,1,3,1,8,7,7,6,6,3,7,5,8,3,5,9,1,8,7,7,5,10,5,3,5,8,4,5,5,1,8,6,9,3,1,7,4,9,6,9,1,7,7,1,6,8,1,9,5,5,4,2,2,8,3,10,2,1,9,8,10,3,5,9,1,8,4,2,1,3,8,5,5,8,1,6,5,3,10,1,8,5,1,6,4,6,2,7,3,10,5,1,5,4,10,4,10,3,9,7,4,6,2,7,10,2,4,4,1,3,1,6,7,9,4,4,7,10,5,10,2,2,10,8,4,1,3,1,1,2,4,4,4,4,4,8,2,6,7,10,10,9,9,2,5,3,2,8,4,5,4,1,7,5,7,9,9,7,5,9,3,2,4,4,10,8,3,6,5,1,6,9,5,7,8,6,8,7,4,10
};
typedef struct {
DE_KalmanResult_TYPE _KalmanGain;//卡尔曼增益
DE_KalmanResult_TYPE _EstimateCovariance;//估计协方差
DE_KalmanResult_TYPE _MeasureCovariance;//测量协方差
DE_KalmanResult_TYPE _OptimumValue;//最优值
}KalmanType;
KalmanType stKalmanParameter;
void vKalmanFilterInit(void){//卡尔曼滤波初始化
stKalmanParameter._EstimateCovariance = 0.03;
stKalmanParameter._MeasureCovariance = 0.2;
stKalmanParameter._OptimumValue = 5;
}
DE_KalmanResult_TYPE _KalmanFilter(DE_KalmanResult_TYPE _MeasureValue){//卡尔曼滤波计算
//计算卡尔曼增益
stKalmanParameter._KalmanGain = stKalmanParameter._EstimateCovariance * \
sqrt(1/(stKalmanParameter._EstimateCovariance * stKalmanParameter._EstimateCovariance + \
stKalmanParameter._MeasureCovariance * stKalmanParameter._MeasureCovariance));
//计算本次滤波最优值
stKalmanParameter._OptimumValue = stKalmanParameter._OptimumValue + \
stKalmanParameter._KalmanGain*(_MeasureValue - stKalmanParameter._OptimumValue);
//更新估计协方差
stKalmanParameter._EstimateCovariance = sqrt(1 - stKalmanParameter._KalmanGain) * \
stKalmanParameter._EstimateCovariance;
//更新测量协方差
stKalmanParameter._MeasureCovariance = sqrt(1 - stKalmanParameter._KalmanGain) * \
stKalmanParameter._MeasureCovariance;
//返回最优值
return stKalmanParameter._OptimumValue;
}
int main ()
{
int b,d=0;
b=sizeof(a)/sizeof(a[1]);
printf("KalmanFilter初始化\n");
vKalmanFilterInit();
printf("KalmanFilter初始化完成\n");
getch();
while(d < 200)
{
printf("%lf\n",_KalmanFilter(a[d++]));
}
printf("结束！！！\n");
getch();
return 0;
}