I tried to make matlab Kalman filter simulation code, but I have a problem, I can't achieve estimated states. I would be greatfull for any help

Ra=2; L=0.003; lambda=0.1; J=0.002; B=0.001;

ControlNoise=0.001; AccelNoise=0.05; MeasNoise=0.1;

R=[MeasNoise^2]; xdotNoise=[ControlNoise/L; 0.5; 0]; Q=[xdotNoise(1)^2 0 0; 0 xdotNoise(2)^2 0; 0 0 xdotNoise(3)^2];

P=1*eye(3); dt=0.0002; T=0.001; tf=2;

x=[0; 0; 0]; xhat=x; w=2*pi; Q=Q*T; dtPlot=0.01; tPlot=-inf;

xArray=[]; xhatArray=[]; trPArray=[]; tArray=[];

for t=0:T:tf-T+eps y=[x(1)]+MeasNoise*randn(1,1); if t>=tPlot+dtPlot tPlot=t+dtPlot-eps; xArray=[xArray x]; xhatArray=[xhatArray xhat]; trPArray=[trPArray trace(P)]; tArray=[tArray t]; end for tau=0:dt:T-dt+eps time=t+tau; ua=sin(w*time); xdot=[-Ra/L*x(1)+x(2)*lambda/L*sin(x(3))+ua/L; -3/2*lambda/J*x(1)*sin(x(3))-B/J*x(2); x(2)]; xdot=xdot+xdotNoise.*randn(3,1); x=x+xdot*dt; x(3)=mod(x(3),2*pi); end ua=sin(w*t); A=[-Ra/L lambda/L*sin(xhat(3)) (xhat(2))*lambda/L*sin(xhat(3)); -3/2*lambda/J*sin(xhat(3)) -B/J -3/2*lambda/J*(xhat(1))*sin(xhat(3)); 0 1 0]; C=[1 0 0]; K=P*C'+inv(C*P*C'+R); deltax=[-Ra/L*xhat(1)+(xhat(2))*lambda/L*sin(xhat(3))+ua/L; -3/2*lambda/J*(xhat(1))*sin(xhat(3))-B/J*(xhat(2)); xhat(2)]*T; xhat=xhat+deltax+K*(y-(xhat(1))); xhat(3)=mod(xhat(3),2*pi); P=A*((eye(3)-K*C)*P)*A'+Q; end close all; figure;set(gcf,'Color','White');

subplot(2,2,1); hold on; box on; plot(tArray, xArray(1,:),'b-','LineWidth',2); plot(tArray, xhatArray(1,:),'r:','LineWidth',2); set(gca,'FontSize',12); ylabel('Current A (Amps)');