function ukf_6_div_system
n=6;%状态维数
t=0.5;%采样时间
Q=diag([1 1 0.01 0.01 0.0001 0.0001]);%过程噪声协方差
R=diag([100 0.001^2]);%观测噪声协方差
f=@(x)[x(1)+t*x(3)+0.5*t^2*x(5);x(2)+t*x(4)+0.5*t^2*x(6);x(3)+t*x(5);x(4)+t*x(6);x(5);x(6)];%状态方程%x(1)x(2)为xy轴的位置；x(3),x(4)分别是xy轴的速度，x(5),x(6)分别是xy轴的加速度
h=@(x)[sqrt(x(1)^2+x(2)^2);atan(x(2)/x(1))];%观测方程
s=[1000;5000;10;50;2;-4];%初始赋值
x0=s+sqrtm(Q)*randn(n,1);
P0=diag([100 100 1 1 0.1 0.1]);
N=50;%仿真步数
Xukf=zeros(n,N);%UKF滤波状态初始化
X=zeros(n,N);%%真是状态
Z=zeros(2,N);%%测量值
for i=1:N;
X(:,i)=f(s)+sqrtm(Q)*randn(6,1);
s=X(:,i);
end
ux=x0;%ux是中间变量
for k=1:N;
Z(:,k)=h(X(:,k))+sqrtm(R)*randn(2,1);
[Xukf(:,k),P0]=ukf(f,ux,P0,h,Z(:,k),Q,R);
ux=Xukf(:,k);
end
%%%以下为误差分析
for k=1:N;
RMS(k)=sqrt((X(1,k)-Xukf(1,k))^2+(X(2,k)-Xukf(2,k))^2);
end
figure %轨迹图
t=1:N;
hold on;box on;
plot(X(1,t),X(2,t),'k-')
plot(Z(1,t).*cos(Z(2,t)),Z(1,t).*sin(Z(2,t)),'b-.')
plot(Xukf(1,t),Xukf(2,t),'r-')
legend('实际值','测量值','ukf估计值')
xlabel('x方向位置/m')
ylabel('y方向位置/m')
figure %误差分析图
box on
plot(RMS,'-ko','MarkerFace','r')
xlabel('t/s');
ylabel('偏差/m')
title('跟踪位置偏差')
%%%%%%%%%%%%%%%%%以下为UKF子程序%%%%
function [X,P]=ukf(ffun,X,P,hfun,Z,Q,R)
L=numel(X);%%状态维数
m=numel(Z);%%%%观测维数
alpha=1e-2;
ki=0;
beta=2;
lambda=alpha^2*(L+ki)-L;
c=L+lambda;
Wm=[lambda/c 0.5/c+zeros(1,2*L)];
Wc=Wm;
Wc(1)=Wc(1)+(1-alpha^2+beta);
c=sqrt(c);
Xsigmaset=sigma(X,P,c);%%sigma点集在状态X附近的点集，X是6*13的矩阵，每列样本为1
[X1means,X1,P1,X2]=ut(ffun,Xsigmaset,Wm,Wc,L,Q);%%对状态进行UT变换
[Zpre,Z1,Pzz,Z2]=ut(hfun,X1,Wm,Wc,m,R); %%5对观测进行ut变换
Pxz=X2*diag(Wc)*Z2'; %%计算协方差Pxz
K=Pxz*inv(Pzz);%%计算卡尔曼增益
X=X1means+K*(Z-Zpre);%%%状态更新
P=P1-K*Pxz';%%协方差更新
%%%%UT变换子函数，输入fun函数句柄，Xsigma为样本集，Wm和Wc为权值，n=6为状态维数，COV为方差，输出Xmeans为均值
function [Xmeans,Xsigma_pre,P,Xdiv]=ut(fun,Xsigma,Wm,Wc,n,COV);
LL=size(Xsigma,2);%得到样本的个数
Xmeans=zeros(n,1);
Xsigma_pre=zeros(n,LL);
for k=1:LL
Xsigma_pre(:,k)=fun(Xsigma(:,k));
Xmeans=Xmeans+Wm(k)*Xsigma_pre(:,k);
end
Xdiv=Xsigma_pre-Xmeans(:,ones(1,LL));%%%%预测减去均值，Xmeans(:,ones(1,LL)将Xmeans扩展成n*LL矩阵，每一列都相等
P=Xdiv*diag(Wc)*Xdiv'+COV %%%协方差
%%%%%%%%%%%%%产生sigma点集函数
function Xset=sigma(X,P,c)
A=c*chol(P)';%choleskey分解
Y=X(:,ones(1,numel(X)));
Xset=[X Y+A Y-A];
