칼만필터

The program implements a stochastic filter known as a Kalman Estimator for "Estimating a Constant " problem. The program implements a Kalman estimator for "Estimating a constant problem.". It simulates a measuring process where the measured data is corrupted by white gaussian noise. It uses no matrix manipulations to maintain simplicity. It ouputs the corrupt input and the filtered ouput into different files for plotting.

/* 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); }

어디선가 주워온 소스

kalman_intro.pdf