无损卡尔曼滤波ukf matlab程序.docx

ukf(无迹卡尔曼滤波)算法的matlab程序.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%% Example:%{n=3;????? %number of stateq=0.1;??? %std of process?r=0.1;??? %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 equationsh=@(x)x(1);?????????????????????????????? % measurement equations=[0;0;1];??????????????????????????????? % initial statex=s+q*randn(3,1); %initial state????????? % initial state with noiseP = eye(n);?????????????????????????????? % initial state covraianceN=20;???????????????????????????????????? % total dynamic stepsxV = zeros(n,N);????????? %estmate??????? % allocate memorysV = zeros(n,N);????????? %actualzV = zeros(1,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);??????????? % ekf?xV(:,k) = x;??????????????????????????? % save estimates = f(s) + q*randn(3,1);??????????????? % update process?endfor k=1:3???????????????????????????????? % plot resultssubplot(3,1,k)plot(1:N, sV(k,:), '-', 1:N, xV(k,:), '--')end%}%% By Yi Cao at Cra