XXa2=(1-exp(-T/a))*a; XXa3=exp(-T/a); a=20000;
YYa1=(T/a-1+exp(-T/a))*a^2; YYa2=(1-exp(-T/a))*a; YYa3=exp(-T/a); a=20;
ZZa1=(T/a-1+exp(-T/a))*a^2; ZZa2=(1-exp(-T/a))*a; ZZa3=exp(-T/a); Hk=[1 0 0 0 0 0 0 0 0; 0 0 0 1 0 0 0 0 0; 0 0 0 0 0 0 1 0 0]; Fk=[1 T XXa1 0 0 0 0 0 0; 0 1 XXa2 0 0 0 0 0 0; 0 0 XXa3 0 0 0 0 0 0; 0 0 0 1 T YYa1 0 0 0 ; 0 0 0 0 1 YYa2 0 0 0; 0 0 0 0 0 YYa3 0 0 0; 0 0 0 0 0 0 1 T ZZa1; 0 0 0 0 0 0 0 1 ZZa2; 0 0 0 0 0 0 0 0 ZZa3];
x=Zk(1,1);y=Zk(2,1);z=Zk(3,1);l=size(Zk,2)+1;
diffy=diff(Zk(2,:),1);yy=diffy(1);diffy=diff(Zk(2,:),2); yyy=diffy(1);
diffx=diff(Zk(1,:),1);xx=diffx(1);diffx=diff(Zk(1,:),2); xxx=diffx(1);
diffz=diff(Zk(3,:),1);zz=diffz(1);diffz=diff(Zk(3,:),2); zzz=diffz(1);
P0=[4e6 0 0 0 0 0 0 0 0; 0 6e7 0 0 0 0 0 0 0; 0 0 4e5 0 0 0 0 0 0; 0 0 0 6e7 0 0 0 0 0; 0 0 0 0 4e9 0 0 0 0; 0 0 0 0 0 3e5 0 0 0; 0 0 0 0 0 0 6e3 0 0; 0 0 0 0 0 0 0 3e4 0; 0 0 0 0 0 0 0 0 8e3 ];
X0=[x xx xxx y yy yyy z zz zzz]';R=eye(3)*sqrt(40/3); Xkk1=zeros(9,l); Pkk=P0;
Xkk=zeros(9,l);Xkk(:,1)=X0; for i=1:l-1
Xkk1(:,i)=Fk*Xkk(:,i); Pkk1=Fk*Pkk*Fk';
35 - -
Kk=Pkk1*Hk'*inv(Hk*Pkk1*Hk'+R);
Xkk(:,i+1)=Xkk1(:,i)+Kk*(Zk(:,i)-Hk*Xkk1(:,i)); Pkk=Pkk1-Pkk1*Hk'*inv(Hk*Pkk1*Hk'+R)*Hk*Pkk1; end figure(1)
plot3(Zk(1,:),Zk(2,:),Zk(3,:),'b:'); hold on
plot3(Xkk(1,:),Xkk(4,:),Xkk(7,:),'r.-'); xlabel('x');ylabel('y');zlabel('z'); hold off grid on
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%二维singer model%%%%%%%%%%%%%%%%%%%%%%%% T=0.5;
Zk=[XX1 YY1]'; a=2e1;
XXa1=(T/a-1+exp(-T/a))*a^2; XXa2=(1-exp(-T/a))*a; XXa3=exp(-T/a); a=9e6;
YYa1=(T/a-1+exp(-T/a))*a^2; YYa2=(1-exp(-T/a))*a; YYa3=exp(-T/a); Hk=[1 0 0 0 0 0 ; 0 0 0 1 0 0 ;]; Fk=[1 T XXa1 0 0 0 ; 0 1 XXa2 0 0 0 ; 0 0 XXa3 0 0 0 ; 0 0 0 1 T YYa1 ; 0 0 0 0 1 YYa2 ; 0 0 0 0 0 YYa3 ;];
x=Zk(1,1);y=Zk(2,1);l=size(Zk,2)+1;
diffy=diff(Zk(2,:),1);yy=diffy(1);diffy=diff(Zk(2,:),2); yyy=diffy(1);
diffx=diff(Zk(1,:),1);xx=diffx(1);diffx=diff(Zk(1,:),2); xxx=diffx(1); P0=[50 1 2 3 4 5; 2 60 2 3 4 5; 1 2 70 3 2 3; 1 2 3 76 2 3; 1 2 1 3 76 6; 2 1 3 2 4 89]; X0=[x xx xxx y yy yyy]';
36 - -
R=eye(2,2)*sqrt(40/2);
Xkk1=zeros(6,l);Xkk=zeros(6,l);Xkk(:,1)=X0; Pkk=P0;
for i=1:l-1
Xkk1(:,i)=Fk*Xkk(:,i); Pkk1=Fk*Pkk*Fk';
Kk=Pkk1*Hk'*inv(Hk*Pkk1*Hk'+R);
Xkk(:,i+1)=Xkk1(:,i)+Kk*(Zk(:,i)-Hk*Xkk1(:,i)); Pkk=Pkk1-Pkk1*Hk'*inv(Hk*Pkk1*Hk'+R)*Hk*Pkk1; end figure(5)
plot(Zk(1,:),Zk(2,:),'b:'); hold on
plot(Xkk(1,:),Xkk(4,:),'r.-'); xlabel('x');ylabel('y'); hold off grid on
% %%%%%%%%%%%%%%%%%%%%%%%%ct model%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
T=1;
Zk=[x3 y3]'; omega=2*pi/130; Hk=[1 0 0 0 ; 0 0 1 0;];
Fk=[1 sin(omega)/omega 0 -(1-cos(omega))/omega ; 0 cos(omega) 0 -sin(omega) ; 0 (1-cos(omega))/omega 1 sin(omega)/omega ; 0 sin(omega) 0 cos(omega) ;]; x=Zk(1,1);y=Zk(2,1);l=size(Zk,2)+1;
diffy=diff(Zk(2,:),1);yy=diffy(1);diffy=diff(Zk(2,:),2); diffx=diff(Zk(1,:),1);xx=diffx(1);diffx=diff(Zk(1,:),2); X0=[x xx y yy ]';Xkk=zeros(4,l);Xkk(:,1)=X0; P0=[40 12 3 1; 12 50 3 4; 2 21 50 4; 2 5 1 50];
R=eye(2,2)*sqrt(40/2); Xkk1=zeros(4,l);l=size(Zk,2); Pkk=P0;
37 - -
for i=1:l-1
Xkk1(:,i)=Fk*Xkk(:,i); Pkk1=Fk*Pkk*Fk';
Kk=Pkk1*Hk'*inv(Hk*Pkk1*Hk'+R);
Xkk(:,i+1)=Xkk1(:,i)+Kk*(Zk(:,i)-Hk*Xkk1(:,i)); Pkk=Pkk1-Pkk1*Hk'*inv(Hk*Pkk1*Hk'+R)*Hk*Pkk1; end figure(4)
plot(Zk(1,:),Zk(2,:),'b:'); hold on
plot(Xkk(1,1:l-1),Xkk(3,1:l-1),'r.'); xlabel('x');ylabel('y'); hold off grid on
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%当前统计模型%%%%%%%%%%%%%%%%%%%%%%%%%%%%% T=1;
Zk=[x3 y3]'; a=2e1;
Gk1=[a*(-T+T^2/a/2+a*(1-exp(-T/a)));T-a*(1-exp(-T/a));1-exp(-T/a)]; XXa1=(T/a-1+exp(-T/a))*a^2; XXa2=(1-exp(-T/a))*a; XXa3=exp(-T/a); a=9e6;
Gk2=[a*(-T+T^2/a/2+a*(1-exp(-T/a)));T-a*(1-exp(-T/a));1-exp(-T/a)]; YYa1=(T/a-1+exp(-T/a))*a^2; YYa2=(1-exp(-T/a))*a; YYa3=exp(-T/a); Hk=[1 0 0 0 0 0 ; 0 0 0 1 0 0 ;]; Fk=[1 T XXa1 0 0 0 ; 0 1 XXa2 0 0 0 ; 0 0 XXa3 0 0 0 ; 0 0 0 1 T YYa1 ; 0 0 0 0 1 YYa2 ; 0 0 0 0 0 YYa3 ;];
x=Zk(1,1);y=Zk(2,1);l=size(Zk,2)+1;
diffy=diff(Zk(2,:),1);yy=diffy(1);diffy=diff(Zk(2,:),2);
38 - -
yyy=diffy(1);
diffx=diff(Zk(1,:),1);xx=diffx(1);diffx=diff(Zk(1,:),2); xxx=diffx(1); P0=[50 1 2 3 4 5; 2 60 2 3 4 5; 1 2 70 3 2 3; 1 2 3 76 2 3; 1 2 1 3 76 6; 2 1 3 2 4 89];
X0=[x xx xxx y yy yyy]'; R=eye(2,2)*sqrt(40/2);
Xkk1=zeros(6,l);Xkk=zeros(6,l);Xkk(:,1)=X0; Pkk=P0;
Qk=0*[1/20 1/8 1/6 0 0 0; 1/8 1/3 1/2 0 0 0; 1/6 1/2 1 0 0 0; 0 0 0 1/20 1/8 1/6; 0 0 0 1/8 1/3 1/2; 0 0 0 1/6 1/2 1]; for i=1:l-1
Xkk1(:,i)=Fk*Xkk(:,i) +cat(1,Gk1*Xkk1(3,i),Gk2*Xkk1(6,i)); Pkk1=Fk*Pkk*Fk'+Qk;
Kk=Pkk1*Hk'*inv(Hk*Pkk1*Hk'+R);
Xkk(:,i+1)=Xkk1(:,i)+Kk*(Zk(:,i)-Hk*Xkk1(:,i))+cat(1,Gk1*Xkk1(3,i),Gk2*Xkk1(6,i));
Pkk=Pkk1-Pkk1*Hk'*inv(Hk*Pkk1*Hk'+R)*Hk*Pkk1; end figure(3)
plot(Zk(1,:),Zk(2,:),'b:'); hold on
plot(Xkk(1,:),Xkk(4,:),'r.-'); xlabel('x');ylabel('y'); hold off grid on
39 - -