UKF-MATLAB-仿真

代码来源转载来源

构建Sigma点的函数

[plain] view plain copy

1. function X = sigmas(x,P,c)  
2. %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%  
3. % Sigma points around reference point  
4. % 构造2n+1个sigma点  
5. % Inputs:  
6. % x: reference point  
7. % P: covariance  
8. % c: coefficient  
9. % Output:  
10. % X: Sigma points  
11. %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%  
12.   
13. A = c*chol(P)';  
14. Y = x(:,ones(1,numel(x)));  
15. X = [x Y+A Y-A];  

UT变换函数

[plain] view plain copy

1. function [y,Y,P,Y1] = ut(f,X,Wm,Wc,n,R)  
2. %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%  
3. % Unscented Transformation  
4. % UT转换函数  
5. % Input:  
6. % f: nonlinear map  
7. % X: sigma points  
8. % Wm: weights for mean  
9. % Wc: weights for covraiance  
10. % n: numer of outputs of f  
11. % R: additive covariance  
12. % Output:  
13. % y: transformed mean  
14. % Y: transformed smapling points  
15. % P: transformed covariance  
16. % Y1: transformed deviations  
17. %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%  
18. L = size(X,2);  
19. y = zeros(n,1);  
20. Y = zeros(n,L);  
21. for k=1:L  
22.     Y(:,k) = f(X(:,k));  
23.     y = y+Wm(k)*Y(:,k);  
24. end  
25. Y1 = Y-y(:,ones(1,L));  
26. P = Y1*diag(Wc)*Y1'+R;  

Unscented kalman Filtering 函数

[plain] view plain copy

1. function [x,P] = ukf(fstate, x, P, hmeas, z, Q, R)  
2. %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%  
3. % UKF Unscented Kalman Filter for nonlinear dynamic systems  
4. % 无损卡尔曼滤波（Unscented Kalman Filter）函数，适用于动态非线性系统  
5. % for nonlinear dynamic system (noises are assumed as additive):  
6. %   x_k+1 = f(x_k) + w_k  
7. %   z_k = h(x_k) + v_k  
8. % w ~ N(0,Q) meaning w is gaussian noise with covariance Q  
9. % v ~ N(0,R) meaning v is gaussian noise with covariance R  
10. % =============================参数说明=================================  
11. % Inputs:   
12. % fstate  -[function]: 状态方程f(x)  
13. %     x   -     [vec]: 状态先验估计 "a priori" state estimate  
14. %     P   -     [mat]: 方差先验估计 "a priori" estimated state covariance  
15. % hmeas   -[function]: 量测方程h(x)  
16. %     z   -     [vec]: 量测数据     current measurement  
17. %     Q   -     [mat]: 状态方程噪声w(t) process noise covariance  
18. %     R   -     [mat]: 量测方程噪声v(t) measurement noise covariance  
19. % Output:  
20. %     x   -     [mat]: 状态后验估计 "a posteriori" state estimate  
21. %     P   -     [mat]: 方差后验估计 "a posteriori" state covariance  
22. % =====================================================================  
23. % By Yi Cao at Cranfield University, 04/01/2008  
24. % Modified by JD Liu 2010-4-20  
25. % Modified by zhangwenyu, 12/23/2013  
26. %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%  
27.   
28. if nargin<7  
29.     error('Not enough inputarguments!');  
30. end  
31.   
32. % 初始化，为了简化函数，求lamda的过程被默认  
33. L = numel(x);                                 %numer of states  
34. m = numel(z);                                 %numer of measurements  
35. alpha = 1e-3;                                 %default, tunable  
36. ki = 0;                                       %default, tunable  
37. beta = 2;                                     %default, tunable  
38.   
39. % UT转换部分  
40. lambda = alpha^2*(L+ki)-L;                    %scaling factor  
41. c = L+lambda;                                 %scaling factor  
42. Wm = [lambda/c 0.5/c+zeros(1,2*L)];           %weights for means  
43. Wc = Wm;  
44. Wc(1) = Wc(1)+(1-alpha^2+beta);               %weights for covariance  
45. c = sqrt(c);  
46. X = sigmas(x,P,c);                            %sigma points around x  
47. [x1,X1,P1,X2] = ut(fstate,X,Wm,Wc,L,Q);       %unscented transformation of process  
48. % X1=sigmas(x1,P1,c);                         %sigma points around x1  
49. % X2=X1-x1(:,ones(1,size(X1,2)));             %deviation of X1  
50. [z1,Z1,P2,Z2] = ut(hmeas,X1,Wm,Wc,m,R);       %unscented transformation of measurments  
51.   
52. % 滤波部分  
53. P12 = X2*diag(Wc)*Z2';                        %transformed cross-covariance  
54. K = P12*inv(P2);  
55. x = x1+K*(z-z1);                              %state update  
56. P = P1-K*P12';                                %covariance update  

附带一个原本程序里面使用的例子，可以学习怎么使用这几个函数。当然，如果你懂得无损卡尔曼滤波的过程和用法，这些或许是不需要的。

[plain] view plain copy

1. %n=3; %number of state  
2. clc;  
3. clear;  
4. n=6;  
5. t=0.2;  
6. q=0.1; %std of process  
7. r=0.7; %std of measurement  
8. Q=q^2*eye(n); % covariance of process  
9. R=r^2; % covariance of measurement  
10. %f=@(x)[x(2);x(3);0.05*x(1)*(x(2)+x(3))]; % nonlinear state equations  
11. 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  
12. h=@(x)[sqrt(x(1)+1);0.8*x(2)+0.3*x(1);x(3);x(4);x(5);x(6)];  
13. % measurement equation  
14. %s=[0;0;1]; % initial state  
15. s=[0.3;0.2;1;2;2;-1];  
16. x=s+q*randn(n,1); %initial state % initial state with noise  
17. P = eye(n); % initial state covraiance  
18. N=20; % total dynamic steps  
19. xV = zeros(n,N); %estmate % allocate memory  
20. sV = zeros(n,N); %actual  
21. zV = zeros(n,N);  
22. for k=1:N  
23.     z = h(s) + r*randn; % measurments  
24.     sV(:,k)= s; % save actual state  
25.     zV(:,k) = z; % save measurment  
26.     [x, P] = ukf(f,x,P,h,z,Q,R); % ukf  
27.     xV(:,k) = x; % save estimate  
28.     s = f(s) + q*randn(n,1); % update process  
29. end  
30. for k=1:4 % plot results  
31.     subplot(4,1,k)  
32.     plot(1:N, sV(k,:), '-', 1:N, xV(k,:), '--',1:N,zV(k,:),'*')  
33. end  

相信有了它们，无论你是做科研仿真，还是一般的用途，都足以用于参考或使用。