FILTRO KALMAN APLICADO A UN MOTOR DC

El siguiente programa es un script en MATLAB para estimar la velocidad de un motor DC.

%DATOS DEL SISTEMA
A=[0.4146 -0.0066;1.4643 0.9916];
B=[0.2802;0.3521];
H=[1 0];
u=12;
%MATRICES DE VARIANZA Y ERRORES
%cambiar este factor por 5, 10, 100 y ver
%como mejora la velocidad estimada
%----------------
factor=1;
%----------------
Q=[0.095 0.1194;0.1194 0.15]/factor;
R=0.04;
err_med_corr_armad=0.2;
err_med_tens_ent=1.1;
%DATOS INICIALES
x_est_apriori=[0;0];
P_apriori=[0 0;0 0];
K=[0;0];
x_est=[0;0];
P=Q;
x=[0;0];
%PARA LA SIMULACION
ia=zeros(1,1000);
w=zeros(1,1000);
t=1:1000;
t=t*0.0001;
%PROCESO ITERATIVO
for i=1:1000
%GENERAMOS LAS MEDICIONES RUIDOSAS
%-----------------------------------------------------------------
ruido_de_proceso=err_med_tens_ent*[0.2802*randn;0.3521*randn];
x=A*x+B*u+ruido_de_proceso;
ruido_mediciones=err_med_corr_armad*randn;
z=H*x+ruido_mediciones;
%-----------------------------------------------------------------
%ECUACIONES DE PREDICCION
%-----------------------------------------------------------------
x_est_apriori=A*x_est+B*u;
P_apriori=A*P*A'+Q;
%-----------------------------------------------------------------
%ECUACIONES DE CORRECCION
%-----------------------------------------------------------------
INNOVA=H*P_apriori*H'+R;
K=P_apriori*H'/INNOVA;
x_est=x_est_apriori+K*(z-H*x_est_apriori);
P=A*P_apriori*A'-A*P_apriori*H'*(1/R)*H*P_apriori*A'+Q;
%-----------------------------------------------------------------
%ALMACENAMOS RESULTADOS DE CADA ITERACION
%-----------------------------------------------------------------
ia(i)=x_est(1);
w(i)=x_est(2);
%-----------------------------------------------------------------
end
%GRAFICAMOS LA VELOCIDAD DEL ROTOR
plot(t,w,'k')


Los resultados obtenidos para un "factor" = 100, se muestra en la siguiente figura:

La señal medida y corrompida con ruido es: