📄 ccs.m
字号:
trajectory3;
x(:,1)=X(:,1); %初始状态向量
R=diag([200^2 10^2 200^2 10^2]);
P=50*eye(6); %初始状态向量协方差
u=[1/3 1/3 1/3]';
a1=1;
a2=1/20;
a3=1/100;
a_max=100;
a_max_minus=-100;
%-----------
F=[1 T T^2/2 0 0 0
0 1 T 0 0 0
0 0 1 0 0 0
0 0 0 1 T T^2/2
0 0 0 0 1 T
0 0 0 0 0 1]; %匀加速模型
%-----------
%CS
F1=[1 T -(1-a1*T-exp(-a1*T))/a1^2 0 0 0
0 1 (1-exp(-a1*T))/a1 0 0 0
0 0 exp(-a1*T) 0 0 0
0 0 0 1 T -(1-a1*T-exp(-a1*T))/a1^2
0 0 0 0 1 (1-exp(-a1*T))/a1
0 0 0 0 0 exp(-a1*T) ];
q11= 1/(2*a1^5)*(1-exp(-2*a1*T)+2*a1*T+2/3*a1^3*T^3-2*a1^2*T^2-4*a1*T*exp(-a1*T));
q12= 1/(2*a1^4)*(exp(-2*a1*T)+1-2*exp(-a1*T)+2*a1*T*exp(-a1*T)-2*a1*T+a1^2*T^2);
q13=1/(2*a1^3)*(1-exp(-2*a1*T)-2*a1*T*exp(-a1*T));
q21=q12;
q22=1/(2*a1^3)*(4*exp(-a1*T)-3-exp(-2*a1*T)+2*a1*T);
q23=1/(2*a1^2)*(exp(-2*a1*T)+1-2*exp(-a1*T));
q31=q13;
q32=q23;
q33=1/(2*a1)*(1-exp(-2*a1*T));
q1=[ q11 q12 q13 0 0 0
q21 q22 q23 0 0 0
q31 q32 q33 0 0 0
0 0 0 q11 q12 q13
0 0 0 q21 q22 q23
0 0 0 q31 q32 q33];
%-----------
F2=[1 T -(1-a2*T-exp(-a2*T))/a2^2 0 0 0
0 1 (1-exp(-a2*T))/a2 0 0 0
0 0 exp(-a2*T) 0 0 0
0 0 0 1 T -(1-a2*T-exp(-a2*T))/a2^2
0 0 0 0 1 (1-exp(-a2*T))/a2
0 0 0 0 0 exp(-a2*T) ];
q11= 1/(2*a2^5)*(1-exp(-2*a2*T)+2*a2*T+2/3*a2^3*T^3-2*a2^2*T^2-4*a2*T*exp(-a2*T));
q12= 1/(2*a2^4)*(exp(-2*a2*T)+1-2*exp(-a2*T)+2*a2*T*exp(-a2*T)-2*a2*T+a2^2*T^2);
q13=1/(2*a2^3)*(1-exp(-2*a2*T)-2*a2*T*exp(-a2*T));
q21=q12;
q22=1/(2*a2^3)*(4*exp(-a2*T)-3-exp(-2*a2*T)+2*a2*T);
q23=1/(2*a2^2)*(exp(-2*a2*T)+1-2*exp(-a2*T));
q31=q13;
q32=q23;
q33=1/(2*a2)*(1-exp(-2*a2*T));
q2=[ q11 q12 q13 0 0 0
q21 q22 q23 0 0 0
q31 q32 q33 0 0 0
0 0 0 q11 q12 q13
0 0 0 q21 q22 q23
0 0 0 q31 q32 q33];
%-----------
F3=[1 T -(1-a3*T-exp(-a3*T))/a3^2 0 0 0
0 1 (1-exp(-a3*T))/a3 0 0 0
0 0 exp(-a3*T) 0 0 0
0 0 0 1 T -(1-a3*T-exp(-a3*T))/a3^2
0 0 0 0 1 (1-exp(-a3*T))/a3
0 0 0 0 0 exp(-a3*T) ];
q11= 1/(2*a3^5)*(1-exp(-2*a3*T)+2*a3*T+2/3*a3^3*T^3-2*a3^2*T^2-4*a3*T*exp(-a3*T));
q12= 1/(2*a3^4)*(exp(-2*a3*T)+1-2*exp(-a3*T)+2*a3*T*exp(-a3*T)-2*a3*T+a3^2*T^2);
q13=1/(2*a3^3)*(1-exp(-2*a3*T)-2*a3*T*exp(-a3*T));
q21=q12;
q22=1/(2*a3^3)*(4*exp(-a3*T)-3-exp(-2*a3*T)+2*a3*T);
q23=1/(2*a3^2)*(exp(-2*a3*T)+1-2*exp(-a3*T));
q31=q13;
q32=q23;
q33=1/(2*a3)*(1-exp(-2*a3*T));
q3=[ q11 q12 q13 0 0 0
q21 q22 q23 0 0 0
q31 q32 q33 0 0 0
0 0 0 q11 q12 q13
0 0 0 q21 q22 q23
0 0 0 q31 q32 q33];
%------------------------------------------
for i=2:length(z)
x1=F1*x(:,i-1);
if x1(3) >= 0
delta(1)=(4-pi)/pi*(a_max-x1(3))^2;
else
delta(1)=(4-pi)/pi*(a_max_minus+x1(3))^2;
end
if x1(6) >= 0
delta(2)=(4-pi)/pi*(a_max-x1(6))^2;
else
delta(2)=(4-pi)/pi*(a_max_minus+x1(6))^2;
end
deltaA=diag([delta(1) delta(1) delta(1) delta(2) delta(2) delta(2)]);
Q=2*a1*deltaA*q1;
P1=F1*P*F1'+Q;
H=[ 1, 0, 0, 0, 0, 0
0, 1, 0, 0, 0, 0
0, 0, 0, 1, 0, 0
0, 0, 0, 0, 1, 0];
S1=H*P1*H'+R;
K=P1*H'*inv(S1);% 增益K
V1=z(:,i)-H*x1;
x1=x1+K*V1;
P1=(eye(6)-K*H)*P1*(eye(6)+K*H)'-K*R*K';
%----------
x2=F2*x(:,i-1);
if x2(3) >= 0
delta(1)=(4-pi)/pi*(a_max-x2(3))^2;
else
delta(1)=(4-pi)/pi*(a_max_minus+x2(3))^2;
end
if x2(6) >= 0
delta(2)=(4-pi)/pi*(a_max-x2(6))^2;
else
delta(2)=(4-pi)/pi*(a_max_minus+x2(6))^2;
end
deltaA=diag([delta(1) delta(1) delta(1) delta(2) delta(2) delta(2)]);
Q=2*a2*deltaA*q2;
P2=F2*P*F2'+Q;
H=[ 1, 0, 0, 0, 0, 0
0, 1, 0, 0, 0, 0
0, 0, 0, 1, 0, 0
0, 0, 0, 0, 1, 0];
S2=H*P2*H'+R;
K=P2*H'*inv(S2);% 增益K
V2=z(:,i)-H*x2;
x2=x2+K*V2;
P2=(eye(6)-K*H)*P2*(eye(6)+K*H)'-K*R*K';
%----------
x3=F3*x(:,i-1);
if x3(3) >= 0
delta(1)=(4-pi)/pi*(a_max-x3(3))^2;
else
delta(1)=(4-pi)/pi*(a_max_minus+x3(3))^2;
end
if x3(6) >= 0
delta(2)=(4-pi)/pi*(a_max-x3(6))^2;
else
delta(2)=(4-pi)/pi*(a_max_minus+x3(6))^2;
end
deltaA=diag([delta(1) delta(1) delta(1) delta(2) delta(2) delta(2)]);
Q=2*a3*deltaA*q3;
P3=F3*P*F3'+Q;
H=[ 1, 0, 0, 0, 0, 0
0, 1, 0, 0, 0, 0
0, 0, 0, 1, 0, 0
0, 0, 0, 0, 1, 0];
S3=H*P3*H'+R;
K=P3*H'*inv(S3);% 增益K
V3=z(:,i)-H*x3;
x3=x3+K*V3;
P3=(eye(6)-K*H)*P3*(eye(6)+K*H)'-K*R*K';
%----------
q(1)=exp(-1/2*V1'*inv(S1)*V1)/sqrt(det(2*pi*S1));
q(2)=exp(-1/2*V2'*inv(S2)*V2)/sqrt(det(2*pi*S2));
q(3)=exp(-1/2*V3'*inv(S3)*V3)/sqrt(det(2*pi*S3));
c=q(1)*u(1)+q(2)*u(2)+q(3)*u(3);
u(1)=q(1)*u(1)/c;
u(2)=q(2)*u(2)/c;
u(3)=q(3)*u(3)/c;
%e:输出X、P
x(:,i)=x1*u(1)+x2*u(2)+x3*u(3);
P=u(1)*(P1+(x1-x(:,i))*(x1-x(:,i))')+u(2)*(P2+(x2-x(:,i))*(x2-x(:,i))')+u(3)*(P3+(x3-x(:,i))*(x3-x(:,i))');
end
%画图观察
% figure(1);
% plot(X(1,:),'b-'),hold on;
% plot(z(1,:),'k:')
% plot(x(1,:),'r:')
% title('横坐标位移');
% xlabel('秒');ylabel('米');
% legend('真实轨迹','测量值','滤波值');
%
% figure(2);
% plot(X(2,:),'b-'),hold on;
% plot(z(2,:),'k:')
% plot(x(2,:),'r:')
% title('横坐标速度');
% xlabel('秒');ylabel('米/秒');
% legend('真实轨迹','测量值','滤波值');
%
% figure(3);
% plot(X(4,:),'b-'),hold on;
% plot(z(3,:),'k:')
% plot(x(4,:),'r:')
% title('纵坐标位移');
% xlabel('秒');ylabel('米');
% legend('真实轨迹','测量值','滤波值');
%
% figure(4);
% plot(X(5,:),'b-'),hold on;
% plot(z(4,:),'k:')
% plot(x(5,:),'r:')
% title('纵坐标速度');
% xlabel('秒');ylabel('米/秒');
% legend('真实轨迹','测量值','滤波值');⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -