UKF应用于GPS-IMU组合导航系统的MATLAB代码

组合导航系统的计算程序代码

function yy=ukf_IMUgps()

%function ukf_IMUgps()

% UKF在IMU/GPS组合导航系统中应用

%

% 以IMU中的位置、速度、姿态误差角、陀螺漂移常值为状态量；

% 以GPS中的位置、速度为观测量。

%

% 7,July 2008.

clc

% Initialise state

global we RN RM g fl deta wg Tt wt d ww v u W Rbl Ta wa

d=0; %验证循环次数

%地球自转角速度we(rad/s):

we=7.292115e-5;

g=9.81; %地球重力加速度（m/s^2）

a=6.378137e+6; %地球长半轴

e2=0.0066944; %地球第一偏心率的平方

%姿态角初始值（r,p,y）

zitai=(pi/180).*[0 2.0282 196.9087];

%姿态误差角

fai=(pi/180).*[1/36 1/36 5/36]; %(100'',100'',500'')

r=zitai(1)+fai(1);

p=zitai(2)+fai(2);

y=zitai(3)+fai(3);

%当地坐标系（l）相对于载体坐标系（b）的转换矩阵：Rbl(在e,n,u坐标系下）Rbl=[cos(r)*cos(y)-sin(r)*sin(y)*sin(p) -sin(y)*cos(p) cos(y)*sin(r)+sin(y)*sin(p)*cos(r) cos(r)*sin(y)+sin(r)*cos(y)*sin(p) cos(y)*cos(p) sin(y)*sin(r)-cos(y)*sin(p)*cos(r) -cos(p)*sin(r) sin(p) cos(p)*cos(r)];

%由转换矩阵Rbl计算四元数的初始值Q0=[q1,q2,q3,q4]'

QQ=[0.5*sqrt(1+Rbl(1,1)+Rbl(2,2)+Rbl(3,3))

0.25*(Rbl(3,2)-Rbl(2,3))/(0.5*sqrt(1+Rbl(1,1)+Rbl(2,2)+Rbl(3,3)))

0.25*(Rbl(1,3)-Rbl(3,1))/(0.5*sqrt(1+Rbl(1,1)+Rbl(2,2)+Rbl(3,3)))

0.25*(Rbl(2,1)-Rbl(1,2))/(0.5*sqrt(1+Rbl(1,1)+Rbl(2,2)+Rbl(3,3)))];

%IMU系统给出的初始值：速度（ve,vn,vu），位置（l,m,h）＝（经度，纬度,高度），姿态误差角fai(e,n,u)

vIMU=[-21.775011 -71.631027 3.10094];

point_IMU=[0.147022986 0.81837365 3122.826];

T=1; %UKF滤波的采样周期（s）

%陀螺常值漂移初始值tuo(e,n,u)(单位：。/s）

tuo=[0 0 0];

%陀螺一阶相关时间Tt(s)

Tt=[60 60 60];

%加速度计常值漂移初始值jiasu(e,n,u)(单位：m/s^2）

jiasu=[0 0 0];

%加速度计一阶反相关时间Ta(s)

Ta=[60 60 60];

%IMU系统的状态向量X

x=[vIMU point_IMU fai tuo jiasu]';

%观测量噪声V的斜方差矩阵

R=[];

%GPS系统的量测值Z(vn,ve,vd,m,l,h)

[z Rz]=gps_tiqushuju;

%Vk的噪声序列方差为：Rk

Rz=(Rz./T);

%陀螺常值漂移wt(e,n,u)

wt=(pi/180).*[1/(3600) 1/(3600) 1/(3600)]; % 1 (。/h)

%陀螺常值漂移误差wtt(e,n,u)

wtt=(pi/180).*[0.08/(3600) 0.08/(3600) 0.08/(3600)]; % 0.08 (。/h)

%加速度计常值漂移wa(e,n,u)

wa=[2e-6 2e-6 2e-6]; % 200μg (2e-6 m/s^2)

%加速度计常值漂移误差waa(e,n,u)

waa=[2e-6 2e-6 2e-6]./4; % 50μg (0.5e-6 m/s^2)

%姿态误差角噪声wg

wg=wt;

%状态向量X的斜方差矩阵

P = eye(length(x))*eps; % note: for stability, P should never be quite zero

%速度误差：（0.1m/s) 位置误差：水平（1m),高度（5m)

u=[0.1 0.1 0.1 0.000474429 0.000474429 2 wg wtt waa]';

%IMU系统在载体坐标系下的比力值输出值fb

fb=[];

%IMU系统中陀螺输出量

wib=[]; %为载体坐标系（b）相对于惯性坐标系（i）的角速度向量

[f w]=IMU_tiqushuju; %IMU系统输出的比力值和角速度

%%%%%%%%%通过GPS观测值计算得到的姿态角

zitaijiao_gps=xlsread('D:\myfile\UKF\kalmanfilter_MATLAB\germany_ukf\原始数据\zitiajiao-gps.xls');

%%%------------------------------------------------------------

%% 循环开始：for 1:n

outx=[];

outp=[];

detajiao=zeros(3,1);

NN =1000;

for i=1:NN

outx=[outx;x'];

outp=[outp;diag(P)'];

%大地子午圈曲率半径：RM

RM=a*(1-e2)/(1-e2*(sin(x(5)))^2)^(2/3);

%地球卯酉圈的曲率半径：RN

RN=a/sqrt(1-e2*(sin(x(5)))^2);

%当地坐标系下的比力值fl

%IMU系统在载体坐标系下的比力值输出值fb转换到当地坐标系（l）下fl

fb=f(i,:)';

%fl=Rbl*(fb+[x(13) x(14) x(15)]'); %初始对准，比力值加上加速度计的测量偏差

fl=Rbl*fb;

%计算IMU的速度、位置输出减去GPS相应的输出值：deta(ve,vn,l,h)

zd=z(i,:)';

deta=x([1 2 5 6],:)-zd([1 2 5 6],:);

%观测值zz,及观测噪声R

zz=z(i+1,:)';

v=Rz(i+1,:)';

%zitai_v=[0.001 0.0266 0.9664]'; %GPS观测值姿态角的误差

zitai_v=[0.00001 7.0983e-004 0.0006]'; %GPS观测值姿态角的误差

v=[v;zitai_v];

R=diag(v.^2);

%%通过GPS观测值，计算得到roll=0 ，pitch=atan(ve/vn)，yaw=asin(h12/s12) ，将姿态误差角加入观测量中进行滤波计算

zz=[zz;detajiao];

%%%%GPS计算得到的姿态角

z_zitai=zitaijiao_gps(i+1,:);

%%IMU系统的力学编排方程和姿态误差方程离散化之后的截断误差：

ve=x(1);vn=x(2);vu=x(3);l=x(4);m=x(5);h=x(6);faie=x(7);fain=x(8);faiu=x(9);tuo1=x(10);tuo2=x (11);tuo3=x(12);

fl1=fl(1);fl2=fl(2);fl3=fl(3);deta1=deta(1); deta2=deta(2); deta3=deta(3); deta4=deta(4);wg1=wg(1);wg2=wg(2);wg3=wg(3);

jiasu1=x(13);jiasu2=x(14);jiasu3=x(15);Ta1=Ta(1);Ta2=Ta(2);Ta3=Ta(3);wa1=wa(1);wt2=wa(2) ;wt3=wa(3);

Q=diag((u).^2);

% predict

[x,P] = predict(x, P,u, Q, T);

% update

[x,P] = update(x, P, zz, R);

%%

xx=x;

x=xx(1:15,1);

u=xx(16:30,1);

%u(1)=u(1)+x(13);

%u(2)=u(2)+x(14);

%u(3)=u(3)+x(15);

%u(7)=x(10);

%u(8)=x(11);

%u(9)=x(12);

PP=P;

P=PP(1:15,1:15);

%利用四元数Q计算转换矩阵Rbl

%首先计算在当地坐标系（l）下的角速度向量wbl

%地固坐标系（e）相对于当地坐标系（l）的转换矩阵：Rel=Rle'

%Rel=[-sin(x(4)) cos(x(4)) 0

% -sin(x(5))*cos(x(4)) -sin(x(5))*sin(x(4)) cos(x(5))

% cos(x(5))*cos(x(4)) cos(x(5))*sin(x(4)) sin(x(5))];

wie=[0 0 we]'; %wie 为地球自转角速度向量

%%%%%%%

omiga12=[];

for k=0:1

wib=w(i+k,:)'+T.*[x(10) x(11) x(12)]'; %初始对准，角速度加上陀螺漂移wiel=[0 we*cos(x(5)) we*sin(x(5))]';

%wiel=Rel*wie;

well=[-x(2)/(RM+x(6)) x(1)/(RN+x(6)) x(1)*tan(x(5))/(RN+x(6))]';

will=wiel+well;

wlb=wib-Rbl'*will;

%四元数的更新

%计算反对称矩阵omiga

omiga=[0 wlb(3) -wlb(2) wlb(1)

-wlb(3) 0 wlb(1) wlb(2)

wlb(2) -wlb(1) 0 wlb(3)

wlb(1) wlb(2) wlb(3) 0];

omiga12=[omiga12,omiga];

end

omiga1=omiga12(1:4,1:4);

omiga2=omiga12(1:4,5:8);

%采用四阶龙格－库塔法数值积分解Q(K+1)

QQ=QQ+(0.5*T.*(omiga1+omiga2)+8^(-1)*T^2.*(omiga1^2+omiga1*omiga2+omiga2^2)+48^(-1)*T^3.*(omiga1^2*omiga2+omiga1*omiga2^2)+384^(-1)*T^4.*(omiga1^2*omiga2^2))*QQ;

%由Q(k+1)可得Rbl(k+1)

Rbl=[QQ(1)^2+QQ(2)^2-QQ(3)^2-QQ(4)^2 2*(QQ(2)*QQ(3)-QQ(1)*QQ(4)) 2*(QQ(2)*QQ(4)+QQ(1)*QQ(3))

2*(QQ(2)*QQ(3)+QQ(1)*QQ(4)) QQ(1)^2-QQ(2)^2+QQ(3)^2-QQ(4)^2 2*(QQ(3)*QQ(4)-QQ(1)*QQ(2))

2*(QQ(2)*QQ(4)-QQ(1)*QQ(3)) 2*(QQ(2)*QQ(1)+QQ(4)*QQ(3)) QQ(1)^2-QQ(2)^2-QQ(3)^2+QQ(4)^2];

%由Rbl(k＋1）可得姿态角，（翻滚角r，俯仰角p，航向角y）k+1

%实际导航系（p系）相对于理想导航系（n系）存在误差角fai(e,n,u),Cpn为校正矩阵

%当方位角为大失准角时

Cpn=[cos(x(9)) -sin(x(9)) x(8)*cos(x(9))+x(7)*sin(x(9))

sin(x(9)) cos(x(9)) x(8)*sin(x(9))-x(7)*cos(x(9))

-x(8) x(7) 1];

%%当三个方向的失准角为小角度时

Cpnn=[1 -x(9) x(8)

x(9) 1 -x(7)

-x(8) x(7) 1];

if abs(x(9))*180*60/pi < 10 %当姿态误差角（u），即方位角失准角小于10’时的情况；

Rbl=Cpnn*Rbl; %小失准角

else

Rbl=Cpn*Rbl; %大失准角

end

zitai0=[atan(-Rbl(3,1)/Rbl(3,3))

asin(Rbl(3,2))

atan(-Rbl(1,2)/Rbl(2,2))];

if Rbl(2,2)<0 & zitai0(3)<0

zitai0(3)=zitai0(3)+pi;

end

if Rbl(2,2)<0 & zitai0(3)>0

zitai0(3)=zitai0(3)-pi;

end

% detajiao=zitai0-z_zitai';

% detajiao(1)=0;

% detajiao=Rbl*detajiao;

% if abs(detajiao(2)) < abs(x(8))

% x(8)=detajiao(2);

% %zitai0(2)=z_zitai(2);

% end

%if abs(detajiao(3)) < abs(x(9))

% x(9)=detajiao(3);

%zitai0(3)=z_zitai(3);

%end

zitai=[zitai;zitai0'];

end

%将协方差转换成标准差

outp=(outp).^(1/2);

%将位置误差中的经度和纬度形式转换成当地坐标系（e,n,u)形式for k=1:NN

%大地子午圈曲率半径：RM

RM=a*(1-e2)/(1-e2*(sin(outx(k,5)))^2)^(2/3);

%地球卯酉圈的曲率半径：RN

RN=a/sqrt(1-e2*(sin(outx(k,5)))^2);

outp(k,4)=(RN+outx(k,6))*cos(outx(k,5))*outp(k,4);

outp(k,5)=(RM+outx(k,6))*outp(k,5);

end

%绘制高度图

figure

plot(outx(:,6),'r-.');

title('UKF计算的高度(germany-data)');

figure

plot((180/pi)*outx(:,8),'r-');

title('UKF姿态误差角pitch（度）(germany-data)');

% 绘制图：水平运动轨迹

figure

plot(outx(:,4),outx(:,5),'b-');

title('UKF Level of Movement(Germany-data)');

xlabel('Longitude（rad）');

ylabel('Latitude（rad）');

%hold on

%gpsweizhi=xlsread('原始数据\sudu-weizhi-gps.xls');

%plot(gpsweizhi(:,4),gpsweizhi(:,5),'r-');

%hold off

%绘制图；UKF速度误差

figure

subplot(3,1,1)

plot(outp(:,1),'b-');

title('UKF Velocity Error(Germany-data)');

ylabel('Ve(m/s)');

subplot(3,1,2)

plot(outp(:,2),'b-');

ylabel('Vn(m/s)');

subplot(3,1,3)

plot(outp(:,3),'b-');

xlabel('t(s）');

ylabel('Vu(m/s)');

%绘制图；UKF位置误差

figure

subplot(3,1,1)

plot(100.*outp(:,4),'b-');

title('UKF Location Error(Germany-data)'); ylabel('x(cm)');

subplot(3,1,2)

plot(100.*outp(:,5),'b-');

ylabel('y(cm)');

subplot(3,1,3)

plot(100.*outp(:,6),'b-');

xlabel('t(s）');

ylabel('z(cm)');

%绘制图；UKF姿态角误差

figure

subplot(3,1,1)

plot(3600*(180/pi)*outp(:,7),'b-');

title('UKF Attitude Error(Germany-data)'); ylabel('roll(second)');

subplot(3,1,2)

plot(3600*(180/pi)*outp(:,8),'b-');

ylabel('pitch(second)');

subplot(3,1,3)

plot(3600*(180/pi)*outp(:,9),'b-');

xlabel('t(s）');

ylabel('yaw(second)');

xlswrite('计算结果\output_x.xls',outx); xlswrite('计算结果\output_p.xls',outp); xlswrite('计算结果\output_zitai.xls',zitai);

%

%

function [x,P] = predict(x, P,u, Q, T)

%进行无迹变换

P = blkdiag(P,Q);

x=[x;u];

% Perform unscented transform

[x,P] = unscented_transform(@predict_model, [], x, P, T); % notice how the additional parameter 'T' is passed to the model

%%%function [y, Y] = unscented_transform(func, dfunc, x,P,varargin):注意其中'dfunc'不知道？P(1:15,1:15)=P(1:15,1:15)+Q;

%

%

function [x,P] = update(x, P, z, R)

[x,P] = unscented_update(@observe_model,@observe_residual, x, P, z, R);

%

function x = predict_model(x, T)

global wt Tt Ta wa Rbl

%计算预报值

% 由于UKF使用的是离散时间非线性模型，

% 因此需要对IMU/GPS组合系统模型进行离散化处理；

% 这里采用四阶Runge-kuta法以数值积分的形式实现。

% Y(k+1)=Y(k)+(y1+2*y2+2*y3+y4)/6

% 其中：y1=f(Y(k))*T; y2=f(Y(k)+y1/2)*T; y3=f(Y(k)+y2/2)*T; y4=f(Y(k)+y3)*T;

[n,N]=size(x);

y=[];

for k=1:N

xx=x(:,k);

%陀螺漂移常值方程

xx(10)=xx(10)*exp(-1/Tt(1))+wt(1);

xx(11)=xx(11)*exp(-1/Tt(2))+wt(2);

xx(12)=xx(12)*exp(-1/Tt(3))+wt(3);

%加速度计漂移常值方程

xx(13)=xx(13)*exp(-1/Ta(1))+wa(1);

xx(14)=xx(14)*exp(-1/Ta(2))+wa(2);

xx(15)=xx(15)*exp(-1/Ta(3))+wa(3);

x1=detaf(xx,T); % T为采样周期

x2=detaf(xx+x1./2,T);

x3=detaf(xx+x2./2,T);

x4=detaf(xx+x3,T);

xx=xx+(x1+2.*x2+2.*x3+x4)./6+[xx(16:30,1);zeros(15,1)];

y=[y,xx]; %由K状态到K＋1状态的预估计

end

x=y;

%%%

function dta=detaf(x,T)

global we RN RM fl g deta wg Rbl wt Tt

%IMU系统的力学编排方程

zhongli=9.7803267714*(1+0.00527094*(sin(x(5)))^2+0.0000232718*(sin(x(5)))^4)-(0.3086*10^ (-5))*x(6);

dve=(2*we*sin(x(5))+x(1)*tan(x(5))/(RN+x(6)))*x(2)-(2*we*cos(x(5))+x(1)/(RN+x(6)))*x(3)+fl (1)+fl(2)*x(9)-fl(3)*x(8)+[Rbl(1,1) Rbl(1,2) Rbl(1,3)]*[x(13) x(14) x(15)]';

dvn=-(2*we*sin(x(5))+x(1)*tan(x(5))/(RN+x(6)))*x(1)-x(2)*x(3)/(RM+x(6))+fl(2)+fl(1)*x(9)-fl( 3)*x(7)+[Rbl(2,1) Rbl(2,2) Rbl(2,3)]*[x(13) x(14) x(15)]';

dvu=(2*we*cos(x(5))+x(1)/(RN+x(6)))*x(1)+(x(2))^2/(RM+x(6))-zhongli+fl(3)+fl(2)*x(7)-fl(1)* x(8)+[Rbl(3,1) Rbl(3,2) Rbl(3,3)]*[x(13) x(14) x(15)]';

dl=x(1)/(cos(x(5))*(RN+x(6)));

dm=x(2)/(RM+x(6));

dh=x(3);

%IMU系统的姿态误差方程

%dfaie=(we*sin(x(5))+x(1)*tan(x(5))/(RN+x(6)))*x(8)-(we*cos(x(5))+x(1)/(RN+x(6)))*sin(x(9)) -x(2)*(1-cos(x(9)))/(RM+x(6))-deta(2)/(RM+x(6))+x(2)*deta(4)/(RM+x(6))^2+[Rbl(1,1) Rbl(1,2) Rbl(1,3)]*[x(10) x(11) x(12)]';

%dfain=-(we*sin(x(5))+x(1)*tan(x(5))/(RN+x(6)))*x(7)-x(2)*sin(x(9))/(RM+x(6))+(we*cos(x(5)

)+x(1)/(RN+x(6)))*(1-cos(x(9)))+deta(1)/(RN+x(6))-x(1)*deta(4)/(RN+x(6))^2-we*sin(x(5))*det a(3)+[Rbl(2,1) Rbl(2,2) Rbl(2,3)]*[x(10) x(11) x(12)]';

%dfaiu=-(we*cos(x(5))+x(1)/(RN+x(6)))*(x(8)*sin(x(9))-x(7)*cos(x(9)))+x(2)*(x(8)*cos(x(9))+x (7)*sin(x(9)))/(RM+x(6))+deta(1)*tan(x(5))/(RN+x(6))-x(1)*deta(4)*tan(x(5))/(RM+x(6))^2+(we *cos(x(5))+x(1)*(sec(x(5)))^2/(RN+x(6)))*deta(3)+[Rbl(3,1) Rbl(3,2) Rbl(3,3)]*[x(10) x(11) x(12)]';

dfaie=(we*sin(x(5))+x(1)*tan(x(5))/(RN+x(6)))*x(8)-(we*cos(x(5))+x(1)/(RN+x(6)))*x(9)+deta( 2)/(RM+x(6))-x(2)*deta(4)/(RM+x(6))^2+x(10);

dfain=-(we*sin(x(5))+x(1)*tan(x(5))/(RN+x(6)))*x(7)-x(2)*x(9)/(RM+x(6))-deta(1)/(RN+x(6))+x (1)*deta(4)/(RN+x(6))^2+we*sin(x(5))*deta(3)+x(11);

dfaiu=(we*cos(x(5))+x(1)/(RN+x(6)))*x(7)+x(2)*x(8)/(RM+x(6))-deta(1)*tan(x(5))/(RN+x(6))+x (1)*deta(4)*tan(x(5))/(RM+x(6))^2-(we*cos(x(5))+x(1)*(sec(x(5)))^2/(RN+x(6)))*deta(3)+x(12); dta=T.*[dve,dvn,dvu,dl,dm,dh,dfaie,dfain,dfaiu,0,0,0,0,0,0]'; % T为采样周期

dta=[dta;zeros(15,1)];

%

%

function z = observe_model(x)

% 观测方程：z=Hx+v,其中H为单位矩阵

global v

[m,M]=size(x);

h=[eye(9),zeros(9,6),zeros(9,15)];

z = h*x+repvec(v,M);

%

function dz = observe_residual(z1, z2)

% Given two observation values, compute their normalised residual.

% Notice, once again, that the model is vectorised in terms of z1 and z2.

dz = z1-z2;

function x = repvec(x,N)

x = x(:, ones(1,N));

%

%

UKF的程序代码

function [y, Y] = unscented_transform(func, dfunc, x, P, varargin)

%function [y, Y] = unscented_transform(func, dfunc, x, P, ...)

%

% Algorithm implemented as described in:

% A New Method for the Nonlinear Transformation of Means and Covariances in Filters and Estimators

% 3, APIRL 2008

%

% INPUTS:

% func - function handle (@myfunc) or string ('myfunc') for non-linear transform.

% dfunc - function handle (or string) for residual between two transformed values: e = mydfunc(y1, y2).

% x, P - initial mean and covariance.

% ... - optional arguments such that 'func' has form: y = myfunc(x, ...).

%

% OUTPUTS:

% y, Y - transformed mean and covariance.

%

% NOTES:

% 1. The unscented filter has two primary advantages over the EKF: (i) it produces a more accurate

% estimate of the first and second moments of a random vector transformed through a non-linear

% function, and (ii) it does not require analytical Jacobians.

%

% 2. The function 'func' is the non-linear function itself and transforms 'x' to 'y'. This

% function may be passed any number of additional parameters.

% eg, y = myfunc(x, p1, p2, p3);

%

% 3. The function 'dfunc' is required to deal with discontinuous functions. Some non-linear

% functions are discontinuous, but their residuals are not equal to the discontinuity. A classic % example is a normalised polar measurement:

% y1 = myfunc(x1, p1, p2, p3); % lets say y1 == pi

% y2 = myfunc(x2, p1, p2, p3); % lets say y2 == -pi

% dy = y1 - y2; % dy == 2*pi -- this is wrong (must be within +/- pi)

% dy = mydfunc(y1, y2); % dy == 0 -- this is correct

% Thus, 'mydfunc' is a function that computes the true residual of y1-y2. If the function 'myfunc'

% is not discontinuous, or has a trivial residual, just pass [] to parameter 'dfunc'.

%

% 4. The functions 'func' and 'dfunc' must be vectorised. That is, they must be able to accept a set of

% states as input and return a corresponding set of results. So, for 'func', the state x will not be a

% single column vector, but a matrix of N column vectors. Similarly, for 'dfunc', the parameters y1 and

% y2 will each be matrices of N column vectors.

%

% EXAMPLE USE:

% [y,Y] = unscented_transform(@myfunc, @mydfunc, x,P, p1, p2, p3);

% [a,B] = unscented_transform('continuous_model', [], x,P);

%

% Tim Bailey 2003, modified 2004, 2005.

% Set up some values

D = length(x); % state dimension

N = D*2 + 1; % number of samples

scale = 3; % want scale = D+kappa == 3

kappa = scale-D;

% Create samples

Ps = chol(P)'.* sqrt(scale);

ss = [x, repvec(x,D)+Ps, repvec(x,D)-Ps];

% Transform samples according to function 'func'

if isempty(dfunc), dfunc = @default_dfunc; end

ys = feval(func, ss, varargin{:}); % compute (possibly discontinuous) transform

base = repvec(ys(:,1), N); % set first transformed sample as the base

delta = feval(dfunc, base, ys); % compute correct residual

ys = base - delta; % offset ys from base according to correct residual

% Calculate predicted observation mean

idx = 2:N;

y = (2*kappa*ys(:,1) + sum(ys(:,idx),2)) / (2*scale);

% Calculate new unscented covariance

dy = ys - repvec(y,N);

Y = (2*kappa*dy(:,1)*dy(:,1)' + dy(:,idx)*dy(:,idx)') / (2*scale);

% Note: if x is a matrix of column vectors, then x*x' produces the sum of outer-products.

%

%

function e = default_dfunc(y1, y2)

e = y1 - y2;

%

%

function x = repvec(x,N)

x = x(:, ones(1,N));

function [x,P] = unscented_update(zfunc, dzfunc, x,P, z,R, varargin)

%[x,P] = unscented_update(zfunc,dzfunc, x,P, z,R, ...)

%

% 3,APIRL 2008

%

% INPUTS:

% zfunc - function handle (@myfunc) or string ('myfunc') for observe model.

% dzfunc - function handle (or string) for observation residual: v = mydfunc(z, z_predict);

% x, P - predict mean and covariance

% z, R - observation and covariance (observation noise is assumed additive)

% ... - optional arguments such that 'zfunc' has the form: z = myfunc(x, ...)

%

% OUTPUTS:

% x, P - updated mean and covariance

%

% NOTES:

% 1. This function uses the unscented transform to compute a Kalman update.

%

% 2. The function 'zfunc' is the non-linear observation function. This function may be passed % any number of additional parameters.

% eg, z = my_observe_model(x, p1, p2);

%

% 3. The residual function 'dzfunc' is required to deal with discontinuous functions. Some non-linear

% functions are discontinuous, but their residuals are not equal to the discontinuity. A classic % example is a normalised angle measurement model:

% z1 = angle_observe_model(x1, p1, p2, p3); % lets say z1 == pi

% z2 = angle_observe_model(x2, p1, p2, p3); % lets say z2 == -pi

% dz = z1 - z2; % dz == 2*pi -- this is wrong (must be within +/- pi)

% dz = residual_model(z1, z2); % dz == 0 -- this is correct

% Thus, 'residual_model' is a function that computes the true residual of z1-z2. If the function % 'zfunc' is not discontinuous, or has a trivial residual, just pass [] to parameter 'dzfunc'.

%

% 4. The functions 'zfunc' and 'dzfunc' must be vectorised. That is, they must be able to accept a set of

% states as input and return a corresponding set of results. So, for 'zfunc', the state x will not be a

% single column vector, but a matrix of N column vectors. Similarly, for 'dzfunc', the parameters z and

% z_predict will be equal-sized matrices of N column vectors.

%

% EXAMPLE USE:

% [x,P] = unscented_update(@angle_observe_model, @residual_model, x,P, z,R, a, b, c); % [x,P] = unscented_update('range_model', [], x,P, z,R);

%

% Tim Bailey 2003, modified 2004, 2005.

global d

d=d+1

% Set up some values

D = length(x); % state dimension

N = D*2 + 1; % number of samples

scale = 3; % want scale = D+kappa == 3

kappa = scale-D;

% Create samples

P=diag(diag(P));

Ps =chol(P)'.* sqrt(scale);

ss = [x, repvec(x,D)+Ps, repvec(x,D)-Ps];

% Transform samples according to function 'zfunc' to obtain the predicted observation samples if isempty(dzfunc), dzfunc = @default_dfunc; end

zs = feval(zfunc, ss, varargin{:}); % compute (possibly discontinuous) transform

zz = repvec(z,N);

dz = feval(dzfunc, zz, zs); % compute correct residual

zs = zz - dz; % offset zs from z according to correct residual

% Calculate predicted observation mean

zm = (kappa*zs(:,1) + 0.5*sum(zs(:,2:end), 2)) / scale;

% Calculate observation covariance and the state-observation correlation matrix

dx = ss - repvec(x,N);

dz = zs - repvec(zm,N);

Pxz = (2*kappa*dx(:,1)*dz(:,1)' + dx(:,2:end)*dz(:,2:end)') / (2*scale);

%Pzz = dz(:,2:end)*dz(:,2:end)' / (2*scale);

Pzz = (2*kappa*dz(:,1)*dz(:,1)' + dz(:,2:end)*dz(:,2:end)') / (2*scale);

% Compute Kalman gain

%%采用平方根滤波算法对滤波误差协方差矩阵进行Cholesky分解，

%%P(k,k-1)=S(k,k-1)*S(k,k-1)',P(k)=S(k)*S(k)'，其中S(k),S(k,k-1)为下三角矩阵，

%%在任意k时刻，以S(k)的递推式代替P(k)的递推式，避免直接计算P(k)从而克服计算发散。S = Pzz+R;

Sc = chol(S); % note: S = Sc*Sc'

Sci = inv(Sc); % note: inv(S) = Sci*Sci'

Kc = Pxz * Sci;

K = Kc * Sci';

%K=Pxz * inv(S);

%[u,s,v]=svd(S);kc=Pxz*inv(v');kcc=kc*inv(s);K=kcc*inv(u);

% Perform update

x = x + K*(z - zm);

P = P - Kc*Kc';

%P = P - K*S*K';

kk=diag(P);

PSD_check = chol(P);

%

%

function x = repvec(x,N)

x = x(:, ones(1,N));

%

%

function e = default_dfunc(y1, y2)

e = y1 - y2;