kalmanfilter.m

基于Matlab编写的用于对数据进行插值分析和傅立叶变换的程序

clear all; close all;


% Initial Conditions
x(:,1) = [0;0.05];      %Our real plant initial condition       
x_(:,1) = [0;0.04];     %Our estimate initial conidition (they might differ)
xc = x_;                %Set the ideal model as we think it should start
P = [0.01 0;            %set initial error covariance for position & frec, both at sigma 0.1, P=diag([sigma_pos_init^2 sigmav_frec_init^2])
     0     0.01];
sigmav = 0.1;           %the covariance coeficient for the position error, sigma
sigmaw = 0.5;           %the covariance coeficient for the frecuency error, sigma
Q = sigmav*sigmav;      %the error covariance constant to be used, in this case just a escalar unit
R = sigmaw*sigmaw;      %the error covariance constant to be used, in this case just a escalar unit

G = [1;0];              %G is the Jacobian of the plant tranfer functions due to the error.
H = [ 1 0];             %H is the Jacobian of the sensor transfer functions due to the variables involved
W = 1;                  %W is the Jacobian of the sensor transfer functions due to the error.

steps = 1000;   %Amount of steps to simulate

% bucle
for i =2:steps          %start @ time=2 
  % the real plant
  x(:,i) = [sin(x(2,i-1)*(i-1)) + randn*sigmav ; x(2,i-1) ];
  z(i) = x(1,i) + randn*sigmaw;

  % blind prediction (just for comparison)
  xc(:,i) = [sin(xc(2,i-1)*(i-1)); xc(2,i-1)];  
  % prediction
  x_(:,i) = [sin(x_(2,i-1)*(i-1)); x_(2,i-1)];
  z_(i) = x_(1,i);

  % compute F
  F = [0 i*cos(x_(2,i)*i);
       0 1];
  
  % Prediction of the plant covariance
  P = F*P*F' + G*Q*G';
  % Innovation Covariance
  S = H*P*H'+R;
  % Kalman's gain
  K = P*H'*inv(S);
  % State check up and update
  x_(:,i) = x_(:,i) + K * (z(i)-z_(i));
  
  % Covariance check up and update
  P = (eye(2)-K*H)*P;
  
  sigmaP(:,i)=sqrt(diag(P)); %sigmap is for storing the current error covariance for ploting pourposes
end

figure(1);clf; hold on;
plot(x(1,:),'-b');                  %plot the real plant behavior
plot(z,'.r');                       %plot the observations over this plant
plot(x_(1,:),'-g');                 %plot the Kalman filter prediction over the plant
plot(xc(1,:),'-m');                 %The original thought of the plant
plot(x_(1,:)+2*sigmaP(1,:),'-g');   %These two are the threshold in witch I'm certain that the plant state is at a given time
plot(x_(1,:)-2*sigmaP(1,:),'-g');


figure(2);clf;hold on;
plot(x(2,:),'-b');                  %Frecuency estimation
plot(x_(2,:),'-g');                 %Frecuency filtered by Kalman