UKF在Matlab下的实现

UKF在Matlab下的实现西安交通大学刘继冬1.实验综述实验综述：：无迹卡尔曼滤波UKF 是重要的非线性滤波方法。

它采用UT 变换的方法，不再近似系统的非线性方程，它仍然用高斯随机变量表示状态分布，不过是用特定选择的样本点加以描述，每个点叫一个高斯点，它从系统状态的概率密度函数中取出；然后，按系统的真实模型演化，得到非线性演化后的σ点，使得样本均值和样本方差是真实均值和真实方差的好的近似。

在这次实验中，实现了基于UKF的滤波方法，并且建立了两种仿真环境进行实验。

2.2.仿真环境设置仿真环境设置仿真环境设置：：（1）仿真仿真环境环境环境11：一维的线性直线匀加速运动，理想的初始速度为0，位置为0，加速度为1。

速度，位置和加速度均有一定的误差，且误差可能较大，误差系数约为0.3。

同时，测量系统只能检测到当前的速度，其余是0，而且干扰较大。

系统的状态方程为：11t 0X X Q 01t 001K K K +=+测量方程为：[]1Z 010X R K K K +=+状态变量X=（x1，x2，x3）T ，其中，x1为位置，x2为速度，x3为加速度。

（2）仿真仿真环境环境环境22：二维平面的匀加速运动，系统的初始状态为：处于（0.3，0.2）位置，初始速度为X轴为1，Y轴为2，加速度为X方向为2，Y方向为-1。

测量方程为非线性。

若简化为线性系统，为了满足能观性，选取可以测量的量不仅为当前的速度，也有当前的位置和加速度。

状态方程为：110t 000010t 00X X Q 00010t 000010000001K K K +=+测量方程为：1Z h(X )R K K K +=+h=[(x(1)+1)^0.5;0.8*x(2)+0.3*x(1);x(3);x(4);x(5);x(6)] 状态变量X=（x1，x2，x3，x4，x5，x6）T ，其中，x1为X轴位置，x2为Y轴位置，x3、x4分别X，Y轴的速度，x5、x6为两方向的加速度。

3.3.实验结果分析实验结果分析实验结果分析：：在matlab下进行仿真实验，得到各个状态变量的仿真数值。

实验结果如下面两个图，其中实线为变量的真实值，星状的点为测量值，虚线为估计值。

（1）仿真环境1的实验结果如下，三部分分别为X三个状态变量的70个周期的仿真结果：由上图可以看出，在线性系统中，尽管只有当前速度变量x2的测量值，没有测量出位置和加速度，系统却可以较好地估计出当前的准确位置和加速度。

（2）仿真环境2的实验结果如下：四个部分分别为前四个状态变量的20个周期的仿真结果，它们分别代表XY两个方向的位置和速度：由上图可以看出，星状的点偏差明显，但两条线却吻合较好。

也就是说，系统的测量方程虽然为非线性测量方程，且在测量方程的随机误差R较大的情况下，系统仍然能很好的估计出位置和速度。

可见UKF在非线性系统估计滤波的实用价值。

综上所述，无论是在线性系统还是非线性测量系统中，如果采用UKF进行系统估计，只要系统是能观测的，那么就可以很好地进行系统滤波估计。

4.4.MATLAB MATLAB MATLAB源代码源代码源代码：：（1）UKF UKF源代码源代码源代码：：function [x,P]=ukf(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: 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%% By Yi Cao at Cranfield University, 04/01/2008% Modified by JD Liu 2010-4-20L=numel(x); %numer of statesm=numel(z); %numer of measurements alpha=1e-3; %default, tunableki=0; %default, tunablebeta=2; %default, tunablelambda=alpha^2*(L+ki)-L; %scaling factorc=L+lambda; %scaling factorWm=[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); %unscentedtransformation 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 measurmentsP12=X2*diag(Wc)*Z2'; %transformed cross-covariance K=P12*inv(P2);x=x1+K*(z-z1); %state updateP=P1-K*P12'; %covariance updatefunction [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 smapling points% P: transformed covariance% Y1: transformed deviationsL=size(X,2);y=zeros(n,1);Y=zeros(n,L);for k=1:LY(:,k)=f(X(:,k));y=y+Wm(k)*Y(:,k);endY1=Y-y(:,ones(1,L));P=Y1*diag(Wc)*Y1'+R;function X=sigmas(x,P,c)%Sigma points around reference point%Inputs:% x: reference point% P: covariance% c: coefficient%Output:% X: Sigma pointsA = c*chol(P)';Y = x(:,ones(1,numel(x)));X = [x Y+A Y-A];（2）输入文件输入文件源代码源代码源代码：：%n=3; %number of stateclc;clear;n=3;t=0.1;q=0.2; %std of processr=0.3; %std of measurementQ=q^2*eye(n); % covariance of processR=r^2; % covariance of measurement%f=@(x)[x(2);x(3);0.05*x(1)*(x(2)+x(3))]; % nonlinear state equationsf=@(x)[x(1)+t*x(2);x(2)+t*x(3);x(3)]; % nonlinear state equationsh=@(x)[0;x(2);0]; % measurement equation%s=[0;0;1]; % initial states=[0;0;1];x=s+q*randn(3,1); %initial state % initial state with noiseP = eye(n); % initial state covraianceN=70; % total dynamic stepsxV = zeros(n,N); %estmate % allocate memorysV = zeros(n,N); %actualzV = zeros(3,N);for k=1:Nz = h(s) + r*randn; % measurmentssV(:,k)= s; % save actual statezV(:,k) = z; % save measurment[x, P] = ukf(f,x,P,h,z,Q,R); % ukfxV(:,k) = x; % save estimates = f(s) + q*randn(3,1); % update processendfor k=1:3 % plot resultssubplot(3,1,k)plot(1:N, sV(k,:), '-', 1:N, xV(k,:), '--',1:N,zV(k,:),'*')end%n=3; %number of stateclc;clear;n=6;t=0.2;q=0.1; %std of processr=0.7; %std of measurementQ=q^2*eye(n); % covariance of processR=r^2; % covariance of measurement%f=@(x)[x(2);x(3);0.05*x(1)*(x(2)+x(3))]; % nonlinear state equationsf=@(x)[x(1)+t*x(3);x(2)+t*x(4);x(3)+t*x(5);x(4)+t*x(6);x(5);x(6)]; %nonlinear state equationsh=@(x)[sqrt(x(1)+1);0.8*x(2)+0.3*x(1);x(3);x(4);x(5);x(6)]; % measurement equation%s=[0;0;1]; % initial states=[0.3;0.2;1;2;2;-1];x=s+q*randn(n,1); %initial state % initial state with noise P = eye(n); % initial state covraianceN=20; % total dynamic stepsxV = zeros(n,N); %estmate % allocate memorysV = zeros(n,N); %actualzV = zeros(n,N);for k=1:Nz = h(s) + r*randn; % measurmentssV(:,k)= s; % save actual statezV(:,k) = z; % save measurment[x, P] = ukf(f,x,P,h,z,Q,R); % ukfxV(:,k) = x; % save estimates = f(s) + q*randn(n,1); % update processendfor k=1:4 % plot resultssubplot(4,1,k)plot(1:N, sV(k,:), '-', 1:N, xV(k,:), '--',1:N,zV(k,:),'*')end。