JackXavier edited section_matlab_begin_verbatium_x__.tex  almost 8 years ago

Commit id: e1760c2090f887cb4b7175bdf3be8c29cbb601bb

deletions | additions      

       

x(1) = 0;  y(1) = b*x(1) + ErrorSensor;  for time = 1 : (Quantity-1)  ErrorModel = normrnd(0,disp_ErrorModel);  ErrorSensor = normrnd(0,disp_ErrorSensor);  x(time+1) = a*x(time) + ControlF*time + ErrorModel;  y(time+1) = b*x(time+1) + ErrorSensor;  end;  Sn(1) = 1;  PredicVar(1) = 1;  x_predic(1) = 0;  y_predic(1) = 0;  for time = 1 : (Quantity-1)  disp(time);  KalmanGain(time) = (b*Sn(time))/((b*b*Sn(time)) + disp_ErrorModel);  disp(KalmanGain(time));  EstimVar(time) = (1 - b*KalmanGain(time))*Sn(time);  disp(EstimVar(time));  PredicVar(time+1) = EstimVar(time) * a * a + disp_ErrorSensor;  disp(PredicVar(time));  Sn(time+1) = a*a*EstimVar(time) + disp_ErrorSensor;   end;  for time = 1 : (Quantity-1)  disp(time);  x_estim(time) = x_predic(time) + KalmanGain(time)*(y(time)-y_predic(time));  disp( x_estim(time));  x_predic(time+1) = (ControlF*time) + a*x_estim(time);  disp( x_predic(time));  y_predic(time+1) = b*x_predic(time+1);  end;  set(0,'DefaultAxesFontSize',14,'DefaultAxesFontName','Times New Roman');  set(0,'DefaultTextFontSize',14,'DefaultTextFontName','Times New Roman');   figure('Units', 'normalized', 'OuterPosition', [0 0 1 1]);  title('Kalmans Filter');  hold on;  plot(k,x,'r','Color',[.7 .1 .1],'LineWidth',2);  plot(k,y,'--','Color',[.1 .7 .1],'LineWidth',2);  plot(k,x_predic,'r','Color',[.1 .1 .7],'LineWidth',2);  legend('coordinate','observation','result',3);  hold off;  \end{verbatium}