clear;
clc;
dx=sqrt(0.05); % 量测方程中,x轴上的方差
dy=sqrt(0.06); % 量测方程中,y轴上的方差
T=1;
N=500/T; % 采样次数500次
alf=1; % 匀速圆周运动占2*pi的比例
thma1=1e-7; % 状态方程中, x方向上的加速度方差
thma2=1e-7; % 状态方程中,y方向上的加速度方差
% 期望信号
sita=0:(alf*2*pi/N):(alf*2*pi-(alf*2*pi/N)); % 角度,500个点
a=4,b=3; %半径
x=a*cos(sita); %期望信号,x轴上的信号
y=b*sin(sita); %期望信号,y轴上的信号
no1=randn(1,N); % 随机白噪声-x轴上 no1
no1=no1-mean(no1);
no1=dx*no1/sqrt(var(no1));
no2=randn(1,N); % 随机白噪声-y轴上 no2
no2=no2-mean(no2);
no2=dy*no2/sqrt(var(no2));
for i=1:N;
zx(i)=x(i)+no1(i);
zy(i)=y(i)+no2(i);
z(:,i)=[zx(i);zy(i)]; % 观测信号
end
%
X_estimate(2,:)=[zx(2),(zx(2)-zx(1))/T,zy(2),(zy(2)-zy(1))/T]; %X(k)的初始值
X_est=X_estimate(2,:);
P_estimate=[dx^2,dx^2/T,0,0; %P(k)的初始值
dx^2/T,(dx^2)*2/(T^2)+thma1,0,0;
0,0,dy^2,dy^2/T;
0,0,dy^2/T,(dy^2)*2/(T^2)+thma2];
x1(1)=zx(1); y1(1)=zy(1); x1(2)=X_estimate(2,1); y1(2)=X_estimate(2,3);
k=3;
while k<=N
z1=z(:,k);
[X_est,P_estimate]=kalman_filter(X_est,P_estimate,z1,k,dx,dy);
X_estimate(k,:)=X_est;
P(k,:)=[P_estimate(1,1),P_estimate(1,2),P_estimate(2,2),P_estimate(3,3),P_estimate(3,4),P_estimate(4,4)];
x1(k)=X_estimate(k,1); %
y1(k)=X_estimate(k,3);
k=k+1;
end
figure(1);
plot(x,y);
title('期望信号');
grid on;
figure(2);
plot(no1,no2);
title('噪声信号');
grid on;
figure(3);
plot(zx,zy);
title('观测信号');
grid on;
figure(4);
plot(x1,y1);
title('滤波后信号');
grid on;
figure(5);
subplot(2,1,1);
plot(x-x1);
title('x误差信号');
grid on;
subplot(2,1,2);
plot(y-y1);
title('y误差信号');
grid on;
figure(6);
plot(x,y,'b-',zx,zy,'ko',x1,y1,'r-*');
axis([-5 5 -5 5]);
grid on;
legend('期望信号','观测信号','滤波轨迹');