平方根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 & z','x2','x3'};
for k=1:3                                 % plot results
    subplot(3,1,k)
    plot(1:N, sV(k,:), 'r-', 1:N, xV(k,:), 'b--')
    ylabel(str{k})
    if k==1,title('Unscented Kalman Filter Example'),end
end
legend('act','est','Location','Best')
xlabel('index')
% [x,P]=ukf(fstate,x,P,hmeas,z,Q,R);

实现平方根UKF的函数：

function [x,P3]=qrukf(fstate,x,P,hmeas,z,Q,R)
% UKF   Unscented Kalman Filter for nonlinear dynamic systems
% [x, P] = ukf(f,x,P,h,z,Q,R) returns state estimate, x and state covariance, P 
% for nonlinear dynamic system (for simplicity, noises are assumed as additive):
%           x_k+1 = f(x_k) + w_k
%           z_k   = h(x_k) + v_k
% where w ~ N(0,Q) meaning w is gaussian noise with covariance Q
%       v ~ N(0,R) meaning v is gaussian noise with covariance R
% Inputs:   f: function handle for f(x)
%           x: "a priori" state estimate
%           P: "a priori" estimated state covariance
%           h: function handle for h(x)
%           z: current measurement
%           Q: process noise covariance 
%           R: measurement noise covariance
% Output:   x: "a posteriori" state estimate
%           P: "a posteriori" state covariance
%
% Example:
%{
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=20;                                     % total dynamic steps
xV = zeros(n,N);          %estmate        % allocate memory
sV = zeros(n,N);          %actual
zV = zeros(1,N);
for k=1:N
  z = h(s) + r*randn;                     % measurements
  sV(:,k)= s;                             % save actual state
  zV(k)  = z;                             % save measurment
  [x, P] = ukf(f,x,P,h,z,Q,R);            % ekf 
  xV(:,k) = x;                            % save estimate
  s = f(s) + q*randn(3,1);                % update process 
end
for k=1:3                                 % plot results
  subplot(3,1,k)
  plot(1:N, sV(k,:), '-', 1:N, xV(k,:), '--')
end
%}
% Reference: Julier, SJ. and Uhlmann, J.K., Unscented Filtering and
% Nonlinear Estimation, Proceedings of the IEEE, Vol. 92, No. 3,
% pp.401-422, 2004. 
%
% By Yi Cao at Cranfield University, 04/01/2008
%
L=numel(x);                                 %numer of states
m=numel(z);                                 %numer of measurements
alpha=1e-3;                                 %default, tunable
ki=0;                                       %default, tunable
beta=2;                                     %default, tunable
lambda=alpha^2*(L+ki)-L;                    %scaling factor
c=L+lambda;                                 %scaling factor
Wm=[lambda/c 0.5/c+zeros(1,2*L)];           %weights for means
Wc=Wm;
Wc(1)=Wc(1)+(1-alpha^2+beta);               %weights for covariance
c=sqrt(c);
X=sigmas(x,P,c);                            %sigma points around x
[x1,X1,P1,X2]=ut(fstate,X,Wm,Wc,L,Q);          %unscented transformation of process
X1=sigmas(x1,P1,c);                         %sigma points around x1
X2=X1-x1(:,ones(1,size(X1,2)));             %deviation of X1
[z1,Z1,P2,Z2]=ut(hmeas,X1,Wm,Wc,m,R);       %unscented transformation of measurments
P12=X2*diag(Wc)*Z2';                        %transformed cross-covariance
K=P12/P2/P2';
x=x1+K*(z-z1);                              %state update
U=K*P2';
P3=cholupdate(P1,U,'-');
end

function [y,Y,P,Y1]=ut(f,X,Wm,Wc,n,R)
%Unscented Transformation
%Input:
%        f: nonlinear map
%        X: sigma points
%       Wm: weights for mean
%       Wc: weights for covraiance
%        n: numer of outputs of f
%        R: additive covariance
%Output:
%        y: transformed mean
%        Y: transformed sampling points
%        P: transformed covariance
%       Y1: transformed deviations

L=size(X,2);
y=zeros(n,1);
Y=zeros(n,L);
for k=1:L                   
    Y(:,k)=f(X(:,k));       
    y=y+Wm(k)*Y(:,k);       
end
Y1=Y-y(:,ones(1,L));
[temp,Sy]=qr([Y1(:,2:end)*sqrt(Wc(2)) sqrtm(R)].',0);
P=cholupdate(Sy,Y1(:,1)*sqrt(Wc(1)));
end

function X=sigmas(x,P,c)
%Sigma points around reference point
%Inputs:
%       x: reference point
%       P: covariance
%       c: coefficient
%Output:
%       X: Sigma points

A = c*P';
Y = x(:,ones(1,numel(x)));
X = [x Y+A Y-A]; 
end

下面是一个效果图

附注：在某些情况下，用P3=cholupdate(P1,U,‘-’);
会出现负定的情况，原因是
[R1,p] = cholupdate(R,x,‘-’) 将不返回错误消息。如果 p 为 0，则 R1 是 A - x*x’ 的 Cholesky 因子。如果 p 大于 0，则 R1 是原始 A 的 Cholesky 因子。如果 p 为 1，则 cholupdate 将失败，因为旧矩阵不是正定矩阵。如果 p 为 2，则 cholupdate 将失败，因为 R 的上三角不是有效的 Cholesky 因子。
这里用了’-‘参数，所以会出现负定。
这个函数实现的功能是
cholupdate(P1’P1-UU’);
所以可以对P1’P1-UU’进行修改，确保其正定。可以参考网址里面的一段小程序https://stats.stackexchange.com/questions/6364/making-square-root-of-covariance-matrix-positive-definite-matlab/6367#6367

参考文献：
R. van der Merwe and E. Wan. The Square-Root Unscented Kalman Filter for State and Parameter- Estimation, 2001.