无迹卡尔曼滤波非线性观测器 MATLAB

注意：此部分内容转自图书《卡尔曼滤波原理及其应用》第108页， 对匀加速直线运动物体进行运动观测。

%%%%%%%%%%%%%
%功能说明：UKF在目标跟踪中的应用
%参数说明：1、状态6维，x方向的速度、速度、加速度；
%			  y方向的位置、速度、加速度；
%		   2、观测信息为距离和速度
%%%%%%%%%%%%%

clc;
clear all;
n=6;    %状态个数
t=0.5;	%采样时间间隔
Q=[1 0 0 0 0 0;
    0 1 0 0 0 0;
    0 0 0.01 0 0 0;
    0 0 0 0.01 0 0;
    0 0 0 0 0.001 0;
    0 0 0 0 0 0.001];   %过程噪声协方差矩阵
R=[100 0;
    0 0.001^2];			%测量噪声协方差矩阵
f=@(x)[x(1)+t*x(3)+0.5*t^2*x(5);x(2)+t*x(4)+0.5*t^2*x(6);...
    x(3)+t*x(5);x(4)+t*x(6);x(5);x(6)];
%x1为X轴位置，x2为Y轴位置，x3、x4分别是X、Y轴的速度
%x5、x6为X、Y方向的加速度
%观测方程
h=@(x)[sqrt(x(1)^2+x(2)^2);atan(x(2)/x(1))]; 
s=[ 100;200; -10;-60;2; 4];

x0= s+sqrtm(Q)*randn(n,1);  %初始状态
P0 =[50 0 0 0 0 0;
    0 50 0 0 0 0;
    0 0 1 0 0 0;
    0 0 0 1 0 0;
    0 0 0 0 0.1 0;
    0 0 0 0 0 0.1];  %协方差初始值
    
N=40;    %采样次数
Xukf= zeros(n,N);  %UKF滤波状态初始化
X=zeros(n,N);      %真实状态
Z=zeros(2,N);	   %测量值
for i = 1:N
    X(:,i)=f(s)+sqrtm(Q)*randn(6,1);   %模拟，产生目标运动真实状态
    s = X(:,i);
end
ux=x0;    %ux为中间变量
for k=1:N
    Z(:,k)= h(X(:,k)) + sqrtm(R)*randn(2,1);  %测量值  %保存观测
    [Xukf(:,k),P0] = ukf(f,ux,P0,h,Z(:,k),Q,R); %调用ukf滤波算法
    ux= Xukf(:,k); 
end
%跟踪误差分析
%这里只分析位置误差，速度、加速度误差分析从略
for k=1:N 
    RMS(k) = sqrt((X(1,k) -Xukf(1,k))^2+(X(2,k) -Xukf(2,k))^2); 
end

%画轨迹图
figure 
t=1:N;
hold on;box on; 
h1=plot( X(1,t),X(2,t),'-r.') 
h2=plot(Z(1,t).*cos(Z(2,t)),Z(1,t).*sin(Z(2,t)),'--b')
h3=plot(Xukf(1,t),Xukf(2,t),'-k*')
scatter(X(1,1),X(2,1),20,'c','*');
text(X(1,1),X(2,1),'Begin');
scatter(X(1,N),X(2,N),30,'m','^');
text(X(1,N),X(2,N),'End');
xlabel('位置x')
ylabel('位置y')
legend([h1, h2, h3],'True Value','Measured Value','UKF Estimate');

%误差分析图

figure
box on;
plot(RMS)
xlabel('时间')
ylabel('残差')


function [X,P]=ukf(ffun,X,P,hfun,Z,Q,R)
%非线性系统中的UKF算法
    L = numel(X);  %状态维数
    m = numel(Z);  %观测维数
    alpha = 1e-2;  %默认系数，参考无迹变换
    ki=0;
    beta=2;
    lambda = alpha^2*(L+ki)-L;
    c = L + lambda;
    Wm = [lambda/c 0.5/c+zeros(1,2*L)];  %权值
    Wc = Wm;
    Wc(1) = Wc(1)+(1-alpha^2 + beta);    %权值
    c =sqrt(c);
	
	%第一步：获取一组Sigma点集
	%Sigma点集，在状态X附近的点集，X是6*13矩阵，每列为1样本
    Xsigmaset = sigmas(X,P,c);
	%第二、三、四步：对Sigma点集进行一步预测，得到均值X1means和方差P1和新Sigma点集X1
	%对状态进行UT变换
    [X1means,X1,P1,X2] = ut(ffun,Xsigmaset,Wm,Wc,L,Q);
	%第五、六步：得到观测预测，Z1为X1集合的预测，Zpre为Z1的均值
	%Pzz为协方差
    [Zpre,Z1,Pzz,Z2] = ut(hfun,X1,Wm,Wc,m,R);  %对观测值UT变换
    Pxz = X2*diag(Wc)*Z2';					   %协方差Pxz
    
	第七步：计算Kalman增益
    K=Pxz*inv(Pzz);		
    
	第八步：状态和方差更新	
    X=X1means+K*(Z-Zpre);  %状态更新
    P=P1-K*Pxz';		   %协方差更新
end


%UT变换子函数
%输入：fun为函数句柄，Xsigma为样本集，Wm和Wc为权值
%    n为状态维数（n=6），COV为方差
%输出：Xmeans为均值

function [Xmeans,Xsigma_pre,P,Xdiv] = ut(fun,Xsigma,Wm,Wc,n,COV)

	LL = size(Xsigma,2);%获取样本个数
	Xmeans=zeros(n,1);  %均值
	Xsigma_pre = zeros(n,LL);
	for k = 1: LL
		Xsigma_pre(:,k) = fun(Xsigma(:,k));  %一步预测
		Xmeans = Xmeans + Wm(k)*Xsigma_pre(:,k);
	end
	%Xmeans(:,ones(1,LL))将Xmeans扩展为n*LL矩阵，每一列相等
	Xdiv = Xsigma_pre - Xmeans(:,ones(1,LL)); %预测减去均值
	P= Xdiv*diag(Wc)*Xdiv'+COV; %协方差

end

%产生Sigma点集的函数
function Xset = sigmas(X,P,c)
    A = c*chol(P)'; %Cholesky分解
    Y = X(:,ones(1,numel(X)));
    Xset=[X Y + A Y-A];
end

注：必须有观测模型，否则性能比较差，实践中可以用多种观测模型，然后选残差最小的模型。