kalman.m

对超声电机摩擦驱动的仿真

% function [E3,deviation2,D1]=kalman
%P0:Initialized deviation matrix
%X0:Initialized states vector
%H:measurement matrix
%T:Transfer matrix
%Q,R:Deviation of system noise and measurement noise
%n: Iteration number
%h: Interval 
P0=[10,0;0,10];
T=[1,1;0,0.5];
H=[1,0];

n=50;
pn2=P0;
X0=[0;0];
for j=1:1:20
R=j/10;
Q=[0,0;0,j/10];
X{1}=X0;
i=2;
r=size(P0);
I=eye(r(1));
real{1}=X{1}+[0;randn];
D{1}=[0;0];
while i<n
real{i}=T*real{i-1}+[0;randn]; % Iteration of state function 
x=T*X{i-1};
pn1=T*pn2*T'+Q;
K=pn1*H'*inv(H*pn1*H'+R);
y(i)=real{i}(1)+randn;
X{i}=x+K*(y(i)-H*x);
pn2=(I-K*H)*pn1;
D{i}=pn2;
i=i+1;
end

for i=1:1:n-1
E(i)=X{i}(1);
realx(i)=real{i}(1);
D3(i)=E(i)-realx(i);
end
E3(j)=sum(D3)/(n-1)
deviation2(j)=D3*D3'/(n-1);
D1(j)=D{49}(1);
end
subplot(3,1,1),plot([0.1:0.1:2],E3,'g.'),ylabel('x1'),xlabel('n'),title('Difference via initialized value of Q R'),grid;
subplot(3,1,2),plot([0.1:0.1:2],deviation2,'m.'),xlabel('n'),title('Deviation via initialized values of Q R'),grid;
subplot(3,1,3),plot([0.1:0.1:2],D1,'g.');xlabel('n'),title('Estimated deviation via intialized values of Q R'),grid;