无线传感器网络目标跟踪代码

4k-Rn

主函数
  % Create sine function
  %
  S1 = [0.2;1.0];
  S2 = [1.0;-0.2];
  sd = 0.1;
  dt = 0.1;
  w = 1;
  T = (0:dt:30);
  X = sin(w*T);
  Y = X + sd*randn(size(X));

  %
  % Initialize KF to values
  %
  %   x = 0
  %   dx/dt = 0
  %
  % with great uncertainty in derivative
  %
  M = [0;0];
  P = diag([0.1 2]);
  R = sd^2;
  H = [1 0];
  q = 0.1;
  F = [0 1;
       0 0];
  [A,Q] = lti_disc(F,[],diag([0 q]),dt);

  %
  % Track and animate
  %
  MM = zeros(size(M,1),size(Y,2));
  PP = zeros(size(M,1),size(M,1),size(Y,2));
  clf;
  clc;
  disp('In this demonstration we estimate a stationary sine signal from noisy measurements by using the classical Kalman filter.');
  disp(' ');
  disp('The filtering results are now displayed sequantially for 10 time step at a time.');
  disp(' ');
  disp('<push any key to proceed to next time steps>');
  
  for k=1:size(Y,2)
    %
    % Track with KF
    %
    [M,P] = kf_predict(M,P,A,Q);
    [M,P] = kf_update(M,P,Y(k),H,R);

    MM(:,k) = M;
    PP(:,:,k) = P;

    %
    % Animate
    %
    if rem(k,10)==1
      plot(T,X,'b--',...
           T,Y,'ro',...
           T(k),M(1),'k*',...
           T(1:k),MM(1,1:k),'k-');
      legend('Real signal','Measurements','Latest estimate','Filtered estimate')
      title('Estimating a noisy sine signal with Kalman filter.');
      drawnow;
      
      pause;
    end
  end

  clc;
  disp('In this demonstration we estimate a stationary sine signal from noisy measurements by using the classical Kalman filter.');
  disp(' ');
  disp('The filtering results are now displayed sequantially for 10 time step at a time.');
  disp(' ');
  disp('<push any key to see the filtered and smoothed results together>')
  pause;  
  %
  % Apply Kalman smoother
  %
  SM = rts_smooth(MM,PP,A,Q);
  plot(T,X,'b--',...
       T,MM(1,,'k-',...
       T,SM(1,,'r-');
  legend('Real signal','Filtered estimate','Smoothed estimate')
  title('Filtered and smoothed estimate of the original signal');
  
  clc;
  disp('The filtered and smoothed estimates of the signal are now displayed.')
  disp(' ');
  disp('RMS errors:');
  %
  % Errors
  %
  fprintf('KF = %.3f\nRTS = %.3f\n',...
          sqrt(mean((MM(1,-X(1,).^2)),...
          sqrt(mean((SM(1,-X(1,).^2)));

function [x,P] = kf_predict(x,P,A,Q,B,u)

  %
  % Check arguments
  %
  if nargin < 3
    A = [];
  end
  if nargin < 4
    Q = [];
  end
  if nargin < 5
    B = [];
  end
  if nargin < 6
    u = [];
  end
  
  %
  % Apply defaults
  %
  if isempty(A)
    A = eye(size(x,1));
  end
  if isempty(Q)
    Q = zeros(size(x,1));
  end
  if isempty(B) & ~isempty(u)
    B = eye(size(x,1),size(u,1));
  end

  %
  % Perform prediction
  %
  if isempty(u)
    x = A * x;
    P = A * P * A' + Q;
  else
    x = A * x + B * u;
    P = A * P * A' + Q;
  end
function [X,P,K,IM,IS,LH] = kf_update(X,P,y,H,R)

  %
  % Check which arguments are there
  %
  if nargin < 5
    error('Too few arguments');
  end

  %
  % update step
  %
  IM = H*X;
  IS = (R + H*P*H');
  K = P*H'/IS;
  X = X + K * (y-IM);
  P = P - K*IS*K';
  if nargout > 5
    LH = gauss_pdf(y,IM,IS);
  end

function [A,Q] = lti_disc(F,L,Q,dt)

  %
  % Check number of arguments
  %
  if nargin < 1
    error('Too few arguments');
  end
  if nargin < 2
    L = [];
  end
  if nargin < 3
    Q = [];
  end
  if nargin < 4
    dt = [];
  end

  if isempty(L)
    L = eye(size(F,1));
  end
  if isempty(Q)
    Q = zeros(size(F,1),size(F,1));
  end
  if isempty(dt)
    dt = 1;
  end

  %
  % Closed form integration of transition matrix
  %
  A = expm(F*dt);

  %
  % Closed form integration of covariance
  % by matrix fraction decomposition
  %
  n   = size(F,1);
  Phi = [F L*Q*L'; zeros(n,n) -F'];
  AB  = expm(Phi*dt)*[zeros(n,n);eye(n)];
  Q   = AB(1:n,/AB((n+1)2*n),;

c习生

谢谢分享    学习