Unscented kalman Filtering 无损卡尔曼滤波的matlab程序

本来想写下原理的，CSDN的博客貌似不可以编辑公式，那就算了，直接放程序好了。

这个程序大部分是从前人那里看来的，向他们表示感谢，它帮助我完成了一个课程论文的仿真。我很喜欢原作者简洁有效的编程风格。这几个程序对于学习最优估计课程的人来说是很需要的。

构建Sigma点的函数

function X = sigmas(x,P,c)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Sigma points around reference point
% 构造2n+1个sigma点
% Inputs:
% x: reference point
% P: covariance
% c: coefficient
% Output:
% X: Sigma points
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

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)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Unscented Transformation
% UT转换函数
% Input:
% f: nonlinear map
% X: sigma points
% Wm: weights for mean
% Wc: weights for covraiance
% n: numer of outputs of f
% R: additive cova