解决ukf在模型不匹配时出现的发散或者不稳定

srukf 代码，经试验可用，可以解决ukf在模型不匹配时出现的发散或者不稳定等问题

function [Xk Pk] = my_srukf（Xk_1Pk_1Zk Pw PvF）
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%  函数功能： 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（11） = lambda/（L+lambda）;
Wc = Wm;
Wc（11） =（ Wm（11） + （1-alpha^2+beta））;
sqWc = sqrt（Wc）;

%%%%%%%%%%%——时间更新——%%%%%%%%%%%%

% step 1: 计算时间更新的sigma点

g

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

     文件       2087  2012-12-10 11:40  my_srukf.m

----------- ---------  ---------- -----  ----

                 2087                    1