Код на языке MATLAB:
clear all;
N=250; % count of times
a = 0.5; % const
b = 0.93; % const
sigmaPsi=1; % interference
sigmaEta=50; % measurement error
k=1:N;
x=k;
x(1)=0; % The initial value
y(1)=x(1) + normrnd(0,sigmaEta);% adding noise with the aid of pseudorandom number generator according to the normal law
%Kalman Filter
xOpt(1) = y(1);
eOpt(1) = sigmaEta;
Value = 100; % for chop
for t=1:(N-1)
x(t+1)=(a*x(t)+sin(t)*t)+normrnd(0,sigmaPsi)
y(t+1)=b*x(t+1)+normrnd(0,sigmaEta) %add error on current measurement
%chop
if y(t+1) >= Value
y(t+1) = Value;
elseif y(t+1) <= -Value
y(t+1) = -Value;
end
eOpt(t+1)=sqrt((sigmaEta^2)*(eOpt(t)^2+sigmaPsi^2)/(sigmaEta^2+eOpt(t)^2+sigmaPsi^2));
K(t+1)=(eOpt(t+1))^2/sigmaEta^2;
xOpt(t+1)=(a*xOpt(t)+sin(t)*t)*(1-K(t+1))+K(t+1)*y(t+1)
end;
plot(k,x,k,y,k,xOpt)