kalman1.c

kalman估值器,为了简洁未使用矩阵计算,应用了kalman滤波,c语言实现

/* Kalman filter for estimating a random constant : i.e.,estimating a scalar
   random voltage. Measurements are corrupted by noise wth 0.1 Rms voltage.
   The process is governed by the difference equation
   x(k+1) = A(k)x(k) + Bu(k) + w(k).  - General equation.
   x(k+1) = x(k) + w(k).-Problem Specific equation.
   p(w) ~ N(0,Q)

   The measurement equation is
   z(k) = H(k)x(k) + v(k).  - General equation.
   z(k) = x(k) + v(k).- Problem Specific equation.
   p(v) ~ N(0,R)

   Time update equations:
		^-       ^
       1.	x(k+1) = x(k).
		_
       2.       P(k+1) = P(k) + Q.

   Measurement update equations:
			  _
       1.       K(k) =    P(k)
		       __________
			_
			P(k) + R
		^        ^-            ^-
       2.       x(k)  =  x(k) + K(z(k)-x(k))
			     _
       3.       P(k)  = (1-K)P(k)
				   ^-
       Q = 1e-5. We can start with x = 0. We can almost choose any value of
       _                    _
       P(k)(but =! 0). Here P(k)=1. Let the random scalar constant which we
       choose to simulate be -0.37727. As said R=0.1.
   */
#include<stdio.h>
#include<stdlib.h>
#include<math.h>
float normal(float std_dev);
int main()
{  int i=0,FLAG1,FLAG2;
   float x_next, x,P_next,P,K,Q,R,z;
   FILE *fp,*fp_input;

   if((fp=fopen("k1.dat","w"))==NULL)
      {perror("\nFile opening error.");
       FLAG1 = 0;
      }

   /* Initialization of variables */
   P = 1;
   Q = 1.0/100000.0;
   R = 0.1*0.1;
   x = 0.0;

  /* Iteration loop */
   fprintf(fp,"x\t");
   fprintf(fp,"z\n");

   while(i<20000)
     { x_next=x;
       P_next=P+Q;
       K=P_next/(P_next+R);
       z=-0.37727+normal(0.1);
       x=x_next+K*(z-x_next);
       P=(1-K)*P_next;
       i++;
       fprintf(fp,"%f\t",x);
       fprintf(fp,"%f\n",z);
       i++;
     }
   return 0;
}
float normal(float std_dev)
{  float uni_rand,sum,mean,nor_rand;
   int i,j,n=12;
   sum=0;
//   mean = 0.0;
   mean = (float)random(32000)/32000;
   for(j=0;j<12;j++)
     {uni_rand=(float)random(32000)/32000;
     sum=sum+ uni_rand;
     }
   nor_rand=mean+ std_dev*((sum-n/2)/(sqrt((float)n/12)));
   return(nor_rand);
}