UKF 程序matlab仿真

本来想写下原理的,CSDN的博客貌似不可以编辑公式,那就算了,直接放程序好了。

这个程序大部分是从前人那里看来的,向他们表示感谢,它帮助我完成了一个课程论文的仿真。我很喜欢原作者简洁有效的编程风格。这几个程序对于学习最优估计课程的人来说是很需要的。

构建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  

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



  • 8
    点赞
  • 60
    收藏
    觉得还不错? 一键收藏
  • 6
    评论
评论 6
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值