无迹卡尔曼滤波算法（UKF）详细推倒及其仿真(matlab)

著名学者Julier等提出近似非线性函数的均值和方差远比近似非线性函数本身更容易，因此提出了基于确定性采样的UKF算法。
该算法的核心思想是：采用UT变换，利用一组Sigma采样点来描述随机变量的高斯分布，然后通过非线性函数的传递，再利用加权统计线性回归技术来近似非x线性函数的后验均值和方差。
相比于EKF，UKF的估计精度能够达到泰勒级数展开的二阶精度。

1. UT变换
2. 采样策略
3. UKF算法流程
4. UKF算法仿真

采样策略：
根据Sigma点采样策略不同，相应的Sigma点以及均值权值和方差权值也不尽相同，因此UT变换的估计精度也会有差异，但总体来说，其估计精度能够达到泰勒级数展开的二阶精度。
为保证随机变量x经过采样之后得到的Sigma采样点仍具有原变量的必要特性，所以采样点的选取应满足：

下面介绍两种经常使用的采样策略：比例采样和比例修正对称采样






至此UKF算法介绍完毕。
UKF算法仿真：
仿真的例子与上一篇EKF仿真的非线性系统相同。
主程序：

clear all;
clc;
tf = 50; 
Q = 10;w=sqrt(Q)*randn(1,tf); 
R = 1;v=sqrt(R)*randn(1,tf);
P =eye(1);
x=zeros(1,tf);
Xnew=zeros(1,tf);
x(1,1)=0.1; 
Xnew(1,1)=x(1,1);
z=zeros(1,tf);
z(1)=x(1,1)^2/20+v(1); 
zjian=zeros(1,tf);
zjian(1,1)=z(1);
linear = 0.5;
for k = 2 : tf 
    % 模拟系统 
    x(:,k) = linear * x(:,k-1) + (2.5 * x(:,k-1) / (1 + x(:,k-1)^2)) + 8 * cos(1.2*(k-1)) + w(k-1); %状态值 
    z(k) = (x(:,k)^2 / 20) + v(k);%观测值
    f=@(x)(linear * x + (2.5 * x / (1 + x^2)) + 8 * cos(1.2*(k-1)));
    h=@(x)(x^2 / 20);
   [Xnew(:,k),P(:,:,k)] = ukf(f,Xnew(:,k-1),P(:,:,k-1),h,z(k),Q,R); 
end
   figure;
   t = 2 : tf;
   plot(t,x(1,t),'b',t,Xnew(1,t),'r:');
   legend('真实值','UKF估计值');

Sigma点集选取（2n+1个）：

function X = sigmas(x,P,c)  % x：参考点，P：协方差，c：系数，X：Sigma点
A = c*chol(P)';  
Y = x(:,ones(1,numel(x)));  
X = [x Y+A Y-A]; 

UT变换：

function [y,Y,P,Y1] = ut(f,X,Wm,Wc,n,R)  %f:非线性函数，X: sigma点 Wm:均值权值 Wc:方差权值
L = size(X,2);  
y = zeros(n,1);  
Y = zeros(n,L);  
for k=1:L  
    Y(:,k) = f(X(:,k));  %非线性传递后结果 r
    y = y+Wm(k)*Y(:,k);  %均值
end  
Y1 = Y-y(:,ones(1,L));  
P = Y1*diag(Wc)*Y1'+R;    %协方差

UKF：

function [x,P] = ukf(fstate, x, P, hmeas, z, Q, R)    
L = numel(x);                                 %状态数量
m = numel(z);                                 %量测数量 
a = 1e-3;                                     %默认  
ki = 0;                                       %默认
beta = 2;                                     %默认  
lambda = a^2*(L+ki)-L;                          
c = L+lambda;                                
Wm = [lambda/c 0.5/c+zeros(1,2*L)];          
Wc = Wm;  
Wc(1) = Wc(1)+(1-a^2+beta);                
c = sqrt(c);  
X = sigmas(x,P,c);                            
[x1,X1,P1,X2] = ut(fstate,X,Wm,Wc,L,Q);                       
[z1,Z1,P2,Z2] = ut(hmeas,X1,Wm,Wc,m,R);          
% 滤波部分  
P12 = X2*diag(Wc)*Z2';                          
K = P12*inv(P2);  
x = x1+K*(z-z1);                                
P = P1-K*P12'; 

仿真结果：

参考文献:CKF及鲁棒滤波在飞行器姿态估计中的应用研究[D]. 黄蔚.哈尔滨工程大学