本来想写下原理的,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];
- 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 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,P] = ukf(fstate, x, P, hmeas, z, Q, R)
- %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
- % UKF Unscented Kalman Filter for nonlinear dynamic systems
- % 无损卡尔曼滤波(Unscented Kalman Filter)函数,适用于动态非线性系统
- % for nonlinear dynamic system (noises are assumed as additive):
- % x_k+1 = f(x_k) + w_k
- % z_k = h(x_k) + v_k
- % 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:
- % fstate -[function]: 状态方程f(x)
- % x - [vec]: 状态先验估计 "a priori" state estimate
- % P - [mat]: 方差先验估计 "a priori" estimated state covariance
- % hmeas -[function]: 量测方程h(x)
- % z - [vec]: 量测数据 current measurement
- % Q - [mat]: 状态方程噪声w(t) process noise covariance
- % R - [mat]: 量测方程噪声v(t) measurement noise covariance
- % Output:
- % x - [mat]: 状态后验估计 "a posteriori" state estimate
- % P - [mat]: 方差后验估计 "a posteriori" state covariance
- % =====================================================================
- % By Yi Cao at Cranfield University, 04/01/2008
- % Modified by JD Liu 2010-4-20
- % Modified by zhangwenyu, 12/23/2013
- %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
- if nargin<7
- error('Not enough inputarguments!');
- end
- % 初始化,为了简化函数,求lamda的过程被默认
- L = numel(x); %numer of states
- m = numel(z); %numer of measurements
- alpha = 1e-3; %default, tunable
- ki = 0; %default, tunable
- beta = 2; %default, tunable
- % UT转换部分
- 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
- %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
相信有了它们,无论你是做科研仿真,还是一般的用途,都足以用于参考或使用。