ghk_tracker.m
上传用户:szahd2008
上传日期:2020-09-25
资源大小:1275k
文件大小:1k
源码类别:

传真(Fax)编程

开发平台:

Matlab

  1. function [residual, estimate] = ghk_tracker (X0, smoocof, inp, npts, T, nvar)
  2. rn = 1.;
  3. % read the intial estimate for the state vector
  4. X = X0; 
  5. theta = smoocof;
  6. %compute values for alpha, beta, gamma
  7. w1 = 1. - (theta^3);
  8. w2 = 1.5 * (1. + theta) * ((1. - theta)^2) / T;
  9. w3 = ((1. - theta)^3) / (T^2);
  10. % setup the transition matrix PHI
  11. PHI = [1. T (T^2)/2.;0. 1. T;0. 0. 1.];
  12. while rn < npts ;
  13.    %use the transition matrix to predict the next state
  14.    XN = PHI * X;
  15.    error = (inp(rn) + normrnd(0,nvar)) - XN(1);
  16.    residual(rn) = error;
  17.    tmp1 = w1 * error;
  18.    tmp2 = w2 * error;
  19.    tmp3 = w3 * error;
  20.    % compute the next state
  21.    X(1) = XN(1) + tmp1;
  22.    X(2) = XN(2) + tmp2;
  23.    X(3) = XN(3) + tmp3;
  24.    estimate(rn) = X(1);
  25.    rn = rn + 1.;
  26. end
  27. return