ukf无损卡尔曼滤波算法.m

function [xP]=ukf（fstatexPhmeaszQR）
% UKF   Unscented Kalman Filter for nonlinear dynamic systems
% [x P] = ukf（fxPhzQR） 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（0Q） meaning w is gaussian noise with covariance Q
%       v ~ N（0R） 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: fanction 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: demo.m
%第一步 参数计算；
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（12*L）];           %weights for means
Wc=Wm;
Wc（1）=Wc（1）+（1-alpha^2+beta）;               %weights for covariance
c=sqrt（c）;

%第二步 计算Sigma点集X
X=sigmas（xPc）;    %sigma points around x （共2*3+1个）

%第三步 时间更新：将Sigma点集X经过非线性函数fstate向前一步传播（预测），并计算以下几个值
%   x1：预测均值（先验）
%   P1：预测方差
%   X1: 传播后点集
%   X2：X1点集与均值的偏差
[x1X1P1X2]=ut（fstateXWm