卡尔曼滤波

double KalmanFilter(const double ResrcData,
					double ProcessNiose_Q,double MeasureNoise_R,double InitialPrediction)
{
	double R = MeasureNoise_R;
	double Q = ProcessNiose_Q;

	static	double x_last;

	double x_mid = x_last;
	double x_now;

	static	double p_last;

	double p_mid ;
	double p_now;
	double kg;	

	x_mid=x_last; //x_last=x(k-1|k-1),x_mid=x(k|k-1)
	p_mid=p_last+Q; //p_mid=p(k|k-1),p_last=p(k-1|k-1),Q=ÔëÉù
	kg=p_mid/(p_mid+R); //kgΪkalman filter£¬RΪÔëÉù
	x_now=x_mid+kg*(ResrcData-x_mid);//¹À¼Æ³öµÄ×îÓÅÖµ
		
	p_now=(1-kg)*p_mid;//×îÓÅÖµ¶ÔÓ¦µÄcovariance	

	p_last = p_now; //¸üÐÂcovarianceÖµ
	x_last = x_now; //¸üÐÂϵͳ״ֵ̬

	return x_now;		
}

static float angle, angle_dot; 		//外部需要引用的变量
//-------------------------------------------------------
static const float Q_angle=0.001, Q_gyro=0.003, R_angle=0.5, dt=0.01;
			//注意：dt的取值为kalman滤波器采样时间;
static float P[2][2] = {
							{ 1, 0 },
							{ 0, 1 }
						};
	
static float Pdot[4] ={0,0,0,0};

static const char C_0 = 1;

static float q_bias, angle_err, PCt_0, PCt_1, E, K_0, K_1, t_0, t_1;
//-------------------------------------------------------
void Kalman_Filter(float angle_m,float gyro_m)			//gyro_m:gyro_measure
{
	angle+=(gyro_m-q_bias) * dt;
	
	Pdot[0]=Q_angle - P[0][1] - P[1][0];
	Pdot[1]=- P[1][1];
	Pdot[2]=- P[1][1];
	Pdot[3]=Q_gyro;
	
	P[0][0] += Pdot[0] * dt;
	P[0][1] += Pdot[1] * dt;
	P[1][0] += Pdot[2] * dt;
	P[1][1] += Pdot[3] * dt;
	
	
	angle_err = angle_m - angle;
	
	
	PCt_0 = C_0 * P[0][0];
	PCt_1 = C_0 * P[1][0];
	
	E = R_angle + C_0 * PCt_0;
	
	K_0 = PCt_0 / E;
	K_1 = PCt_1 / E;
	
	t_0 = PCt_0;
	t_1 = C_0 * P[0][1];

	P[0][0] -= K_0 * t_0;
	P[0][1] -= K_0 * t_1;
	P[1][0] -= K_1 * t_0;
	P[1][1] -= K_1 * t_1;
	
	
	angle	+= K_0 * angle_err;
	q_bias	+= K_1 * angle_err;
	angle_dot = gyro_m-q_bias;
}

clc;
clear

%%%     观测值 噪声 定义  比如超生波 噪声主要是观测噪声
N=800;
real=linspace(4,5,N);

z=real; % 观测值

w=0.5*randn(1,N); %观测噪声
z=z+w; % 加上观测噪声后的观测值
R=(std(w))^2; % 观测噪声的协方差

v=0.01*randn(1,N);  % 定义过程噪声
Q=(std(v))^2;   %由过程噪声求出协方差
%%%  变量初始化
p(1)=1;
x(1)=0;
%%%  卡尔曼滤波程序   其中A=1 U=0 H=1
for t=2:N;
    x_(t)=x(t-1);
    p_(t)=p(t-1)+Q;

    k(t)=p_(t)/(p_(t)+R);
    x(t)=x_(t)+k(t)*(z(t)-x_(t));
    p(t)=(1-k(t))*p_(t);
end

t=1:N;
plot(t,z,‘b‘,t,x,‘r‘,t,real,‘k‘),legend(‘观测值‘,‘滤波后‘,‘实际值‘);