%%%%—————————————UKF计算平抛算例——————————————
%找不跟踪原因
%尝试不用子程序的编写
%作者：MickeyXu
%日期：2013.9.1
%——————————————————————————————————
%——————————————清屏——————————————————
close all;clear all;
clc;%tic;%tic:计时函数，与toc一起用
%——————————————初始化—————————————————
kx=0.01;
ky=0.05;%x,y方向上的阻尼系数
g=9.81;%重力加速度
t=15;%仿真时间
Ts=0.1;%采样周期
N=fix(t/Ts);%仿真步数
X=zeros(4,N);%状态量记录
X(:,1)=[0;50;500;0];%状态初值
n=4;%系统状态维数
x0=X(:,1);
P0=100*eye(n);%方差初值
%——————————————生成真值————————————————
F=@(x)[x(1)+x(2)*Ts;x(2)+(-kx*x(2)^2)*Ts;x(3)+x(4)*Ts;x(4)+(ky*x(4)^2-g)*Ts];%状态转移函数
H=@(x)[sqrt(x(1)^2+x(3)^2);atan(x(1)/x(3))];%量测函数
dax=1.5;
day=1.5;
dr=10;%距离量测噪声
dalfa=0.01;%角度量测噪声
Qk=diag([0;dax;0;day])^2;%状态噪声协方差矩阵
Rk=diag([dr;dalfa])^2;%量测噪声协方差矩阵
x=[];
X_guji=[];
P_K=cell(1,N);
P_K1=cell(1,N);
P_Z=cell(1,N);
P_XZ=cell(1,N);
K_K=cell(1,N);
for k=2:N
x=X(1,k-1);%横坐标
vx=X(2,k-1);%横轴速度
y=X(3,k-1);%纵坐标
vy=X(4,k-1);%纵轴速度
x=x+vx*Ts;%横坐标更新
vx=vx+(-kx*vx^2+dax*randn(1))*Ts;%横向速度更新
y=y+vy*Ts;%纵坐标更新；
vy=vy+(ky*vy^2-g+day*randn(1))*Ts;%纵向速度更新
X(:,k)=[x;vx;y;vy];
end
%——————————————————————————————————
Z=[];
for k=1:N
r=sqrt(X(1,k)^2+X(3,k)^2)+dr*randn(1);%测距
alfa=atan(X(1,k)/X(3,k))+dalfa*randn(1);%测角度
Z(:,k)=[r;alfa];%记录
end
%————————————滤波算法——————————————————
%UT变换求f_k-1(.)_x_k-1=x_k-1(hat)等
%初值，生成sigma点
%x_k=x0;
x_k=[0;48;480;0];
P_k=P0;
for i=1:N
%记录
%采样策略为对称采样
%kappa=0;%kappa:比例系数，用来调节sigma点和x_ba之间的距离
alf=1e-2;
beta=2;
lamda=3*alf^2-n;
L=2*n;
Wm=[lamda/(n+lamda) 0.5/(n+lamda)+zeros(1,L)];%一阶权重
Wc=Wm;%二阶权重
Wc(1)=Wc(1)+1+beta-alf^2;
sigma=x_k;%x0?
Ps=sqrt(n+lamda)*chol(P_k);
%——————————构造sigma点————————————————
for k=1:L
if k<=n
sigma=[sigma x_k+Ps(:,k)];%用扩维的方法生成后n个点
else
sigma=[sigma x_k-Ps(:,k-n)];%扩维生成后n个点
end
end
%————————————————————————————————
%—————————时间传播———————————————————
x_k1=0;%x_k|k-1估计,先验估计
P_k1=0;%P_k|k-1估计，先验估计
for ks=1:L+1%跑遍所有sigma点
gamma(:,ks)=F(sigma(:,ks));%传播
x_k1=Wm(ks)*gamma(:,ks)+x_k1;%x均值累加，估计x_ba
end
%估计Pk
for kp=1:L+1%跑遍所有sigma点
P_k1=Wc(kp)*(gamma(:,kp)-x_k1)*(gamma(:,kp)-x_k1)'+P_k1;%先验方差累积
end
P_k1=P_k1+Qk;%先验方差计算
%—————————状态传播——————————————————
%—————————构造sigma点————————————————
kexi=x_k1;
Pske=sqrt(n+lamda)*chol(P_k1);
for k=1:L
if k<=n
kexi=[kexi x_k1+Pske(:,k)];
else
kexi=[kexi x_k1-Pske(:,k-n)];
end
end
%————————————————————————————————
z_k1=0;%z_k_k-1估计
P_z=0;%P_z~估计
P_xz=0;%P_x~z~估计
for kx=1:L+1%跑遍所有kexi点
chi(:,kx)=H(kexi(:,kx));%量测传播
z_k1=z_k1+Wm(kx)*chi(:,kx);%量测均值累加，估计z
end
for kpz=1:L+1
P_z=Wc(kpz)*(chi(:,kpz)-z_k1)*(chi(:,kpz)-z_k1)'+P_z;
end
P_z=P_z+Rk;
for kxz=1:L+1
P_xz=Wc(kxz)*(gamma(:,kxz)-x_k1)*(chi(:,kxz)-z_k1)'+P_xz;
end
K_k=P_xz/P_z;%修正系数；
x_k=x_k1+K_k*(Z(:,i)-z_k1);%滤波值
P_k=P_k1-K_k*P_z*K_k';%误差协方差阵更新
K_K{i}=K_k;
P_Z{i}=P_z;
P_XZ{i}=P_xz;
P_K1{i}=P_k1;
X_guji(:,i)=x_k;
P_K{i}=P_k;
end
%————————————无迹卡尔曼滤波——————————————
%x_guji:状态向量估计值
%P:滤波方差？
%systemfun:状态函数
%measuref:量测函数
%xc0:估计初值，x_k~
%z:系统测量值
%p0:初始协方差
%———————————UT变换——————————————————
%———————————画图—————————————————————
T=1:N;%步数序列
%error=x-X_guji;
%{
for k=1:3
subplot(3,1,k);
plot(T,x(k,:),'x-r',T,X_guji(k,:),'x-k');
legend('真实值','滤波后');
xlabel('步数');
%ylabel('x3');
title('Unscented Kalman Filter');
end
%}
figure;
plot(X(1,:),X(3,:),'k');
hold on;
plot(X_guji(1,:),X_guji(3,:),'x-r');
xlabel('X');
ylabel('Y');
legend('理论轨迹','UKF');
figure;
error=X-X_guji;
plot(1:N,error(1,:),'+r',1:N,error(3,:),'xb');
%——————————————————————————————————
