ckf4.rar_滤波算法_目标跟踪 容积卡尔曼滤波算法_非线性滤波

%% ckf算法跟踪(四维状态,2D跟踪） clear all delta_w=1e-3; %过程噪声 delta_v=5; %观测噪声 T=1; t=10; N=t/T; %时间小的时候可以跟踪上，但是到后面就会发散，使协方差矩阵伪负定，便无法跟踪了。这也是标准ckf的弊端，就是无法确保 %迭代之后的协方差为对称正定。 L=4; x0=10; y0=20; location=[x0;y0]; x=zeros(4,N); y=zeros(1,N); F=[1 T 0 0;0 1 0 0;0 0 1 T;0 0 0 1]; G=[T^2/2 0;T 0;0 T^2/2;0 T]; Q=delta_w*eye(2); R=delta_v; x(:,1)=[100,3,80,5]'; x_ckf=zeros(4,N); %真实值 for k=2:N x(:,k)=F*x(:,k-1)+G*sqrt(delta_w)*randn(2,1); end %量测值 for k=1:N y(k)=sqrt((x(1,k)-location(1))^2+(x(3,k)-location(2))^2)+sqrtm(R); end x_ckf(:,1)=x(:,1); P0=eye(4); p=P0; for k=2:N s=chol(p); %状态方程式线性的，不需要用容积变换采样点集 for i=1:2*L sigma=[1 0 0 0 -1 0 0 0; 0 1 0 0 0 -1 0 0; 0 0 1 0 0 0 -1 0; 0 0 0 1 0 0 0 -1]; x1(:,i)=s*sigma(:,i)+x_ckf(:,k-1); x2(:,i)=F*x1(:,i); end x_pre=0; for i=1:2*L x_pre=x_pre+1/(2*L)*x2(:,i); end p_pre=0; for i=1:2*L p_pre=p_pre+1/(2*L)*(x2(i)-x_pre)*(x2(i)-x_pre)'+G*Q*G'; end % x_pre=F*x_ckf(:,k-1); % p_pre=F*p*F'+G*Q*G'; %观测更新用容积变换 s1=chol(p_pre); y11=zeros(1,2*L); for i=1:2*L sigma=[1 0 0 0 -1 0 0 0; 0 1 0 0 0 -1 0 0; 0 0 1 0 0 0 -1 0; 0 0 0 1 0 0 0 -1]; x11(:,i)=s1*sigma(:,i)+x_pre; y11(i)=sqrt((x11(1,i)-location(1))^2+(x11(3,i)-location(2))^2); end y_pre=0; for i=1:2*L y_pre=y_pre+1/(2*L)*y11(i); end pyy1=0; pxy=0; for i=1:2*L pyy1=pyy1+1/(2*L)*(y11(i)-y_pre)*(y11(i)-y_pre)'; pxy=pxy+1/(2*L)*(x11(:,i)-x_pre)*(y11(i)-y_pre)'; end pyy=pyy1+R; K=pxy*inv(pyy); x_ckf(:,k)=x_pre+K*(y(k)-y_pre); p=p_pre-K*pyy*K'; end figure plot(x(1,:),x(3,:),'r'); hold on plot(x_ckf(1,:),x_ckf(3,:),'b');