4.MATLAB源代码：
（1）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
L=numel(x); %状态向量的个数
m=numel(z); %测量状态向量的个数
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(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); %unscented
transformation 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 measurments
P12=X2*diag(Wc)*Z2'; %transformed cross-covariance
K=P12*inv(P2);
x=x1+K*(z-z1); %state update
P=P1-K*P12'; %covariance update
function [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 deviations
L=size(X,2);
y=zeros(n,1);
Y=zeros(n,L);
for k=1:L
Y(:,k)=f(X(:,k));
y=y+Wm(k)*Y(:,k);
end
Y1=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 points
A = c*chol(P)';
Y = x(:,ones(1,numel(x)));
X = [x Y+A Y-A];
（2）输入文件源代码：
%n=3; %number of state
clc;
clear;
n=3;
t=0.1;
q=0.2; %std of process
r=0.3; %std of measurement
Q=q^2*eye(n); % covariance of process
R=r^2; % covariance of measurement
%f=@(x)[x(2);x(3);0.05*x(1)*(x(2)+x(3))]; % nonlinear state equations
f=@(x)[x(1)+t*x(2);x(2)+t*x(3);x(3)]; % nonlinear state equations
h=@(x)[0;x(2);0]; % measurement equation
%s=[0;0;1]; % initial state
s=[0;0;1];
x=s+q*randn(3,1); %initial state % initial state with noise
P = eye(n); % initial state covraiance
N=70; % total dynamic steps
xV = zeros(n,N); %estmate % allocate memory
sV = zeros(n,N); %actual
zV = zeros(3,N);
for k=1:N
z = h(s) + r*randn; % measurments
sV(:,k)= s; % save actual state
zV(:,k) = z; % save measurment
[x, P] = ukf(f,x,P,h,z,Q,R); % ukf
xV(:,k) = x; % save estimate
s = f(s) + q*randn(3,1); % update process
end
for k=1:3 % plot results
subplot(3,1,k)
plot(1:N, sV(k,:), '-', 1:N, xV(k,:), '--',1:N,zV(k,:),'*')
end
%n=3; %number of state
clc;
clear;
n=6;
t=0.2;
q=0.1; %std of process
r=0.7; %std of measurement
Q=q^2*eye(n); % covariance of process
R=r^2; % covariance of measurement
%f=@(x)[x(2);x(3);0.05*x(1)*(x(2)+x(3))]; % nonlinear state equations
f=@(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 equations
h=@(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 state
s=[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 covraiance
N=20; % total dynamic steps
xV = zeros(n,N); %estmate % allocate memory
sV = zeros(n,N); %actual
zV = zeros(n,N);
for k=1:N
z = h(s) + r*randn; % measurments
sV(:,k)= s; % save actual state
zV(:,k) = z; % save measurment
[x, P] = ukf(f,x,P,h,z,Q,R); % ukf
xV(:,k) = x; % save estimate
s = f(s) + q*randn(n,1); % update process
end
for k=1:4 % plot results
subplot(4,1,k)
plot(1:N, sV(k,:), '-', 1:N, xV(k,:), '--',1:N,zV(k,:),'*')
end
