椭圆运动_椭圆运动轨迹_卡尔曼滤波_椭圆_

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('期望信号','观测信号','滤波轨迹');