平方根UKF算法

在UKF算法程序的基础上（源程序位于[https://www.mathworks.com/matlabcentral/fileexchange/18217-learning-the-unscented-kalman-filter?s_tid=srchtitle]）。参考平方根算法，进行了修改，结果和UKF基本完全一样。
调用的函数如下：

% test_ukf.m
clc,close all,clear all,dbstop if error
n=3;      %number of state
q=0.1;    %std of process
r=0.1;    %std of measurement
Q=q^2*eye(n); % covariance of process
R=r^2;        % covariance of measurement
f=@(x)[x(2);x(3);0.05*x(1)*(x(2)+x(3))];  % nonlinear state equations
h=@(x)x(1);                               % measurement equation
s=[0;0;1];                                % initial state
x=s+q*randn(3,1); %initial state          % initial state with noise
P = eye(n);                               % initial state covraiance
N=200;                                     % total dynamic steps
xV = zeros(n,N);          %estimate       % allocate memory
sV = zeros(n,N);          %actual
zV = zeros(1,N);          % measurement
tic;
for k=1:N
    z = h(s) + r*randn;                     % measurements
    sV(:,k)= s;                             % save actual state
    zV(k)  = z;                             % save measurment
    [x, P] = qrukf(f,x,P,h,z,Q,R);
%     [x, P] = ukf(f,x,P,h,z,Q,R);
    xV(:,k) = x;                            % save estimate
    s = f(s) + q*randn(3,1);                % update process
end
toc;
str = {'x1 &