function [Xk, Pk] = my_srukf(Xk_1,Pk_1,Zk, Pw, Pv,F)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 函数功能： SRUKF滤波器
% 适用范围： 测量线性、噪声加性时的SRUKF 滤波
% INPUT
% x1： 系统状态
% T： 捷联矩阵计算值
% Tbn： 捷联矩阵真实值
% fp： 比力在计算坐标系内的投影
% Pw： 过程噪声
% Pv： 量测噪声
% phi： 计算的纬度
% OUTPUT
% Xk: 滤波后的状态
% Pk: 状态协方差阵
%
% 注：里边的参数选的比较特殊：alpha = 1;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%——参数计算——%%%%%%%%%%%%%%%%%%
alpha = 1;
beta = 2;
kappa =-5;
L = length(Xk_1);
T=length(Zk);
lambda = alpha^2*(L+kappa) - L;
Wm = repmat(1/(2*(L+lambda)), 2*L+1, 1);
Wm(1,1) = lambda/(L+lambda);
Wc = Wm;
Wc(1,1) =( Wm(1,1) + (1-alpha^2+beta));
sqWc = sqrt(Wc);
%%%%%%%%%%%——时间更新——%%%%%%%%%%%%
% step 1: 计算时间更新的sigma点
gamma = sqrt(L+lambda);
c = gamma*(Pk_1)';
rXk_1 = repmat(Xk_1,1,L);
sigma = [Xk_1, rXk_1+c, rXk_1-c];
% step 2: 计算时间更新方程
Xkk_1 = zeros(L,1);
Ykk_1=zeros(T,1);
for k=1:1:2*L+1
sigmakk_1(:,k) = F*sigma(:,k);
ytt1(:,k)=[acos((sigmakk_1(1,k)/sqrt(sigmakk_1(1,k)^2+sigmakk_1(3,k)^2))),acos((sigmakk_1(1,k))/sqrt((sigmakk_1(1,k))^2+sigmakk_1(3,k)^2))]';
Ykk_1 = Ykk_1 + Wm(k)*ytt1(:,k);
Xkk_1 = Xkk_1 + Wm(k)*sigmakk_1(:,k);
end
for k=2:2*L+1
xeerr(:,k-1) = ( sqWc(k)*(sigmakk_1(:,k)-Xkk_1) );
yeerr(:,k-1) = ( sqWc(k)*(ytt1(k)-Ykk_1) );
end
[aaa,Sx] = qr( [xeerr Pw ]',0); %%%（求出的是上三角，需要的是下三角）
[bbb,Sy] = qr( [yeerr Pv ]',0);
Sx = cholupdate( Sx, sqWc(1,1)* ( sigmakk_1(:,1)-Xkk_1 ));
Sy = cholupdate( Sy, (sqWc(1,1)*(ytt1(:,1)-Ykk_1)) );
Pxy=zeros(L,T);
for k=1:1:2*L+1
xrr = (sigmakk_1(:,k)-Xkk_1);
yrr = ytt1(:,k)-Ykk_1;
Pxy= Pxy + Wc(k)*xrr*yrr';
end
% step 3: 量测更新
Kk = (Pxy/Sy)/(Sy') ;
Xk = Xkk_1+Kk*(Zk-Ykk_1);
UU = Kk*Sy';
Sx = cholupdate( (Sx),UU(:,1),'-');
Pk = Sx;
%%%%%%%%%%%%%%%%——end——%%%%%%%%%%%%%%%%
