大学生考试网 让学习变简单
当前位置:首页 >> 理学 >>

无损卡尔曼滤波UKF Matlab程序

无损卡尔曼滤波UKF Matlab程序

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 f: function handle for f(x)

% Inputs: % % % % % %

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 x: "a posteriori" state estimate

% Output: % % % Example: %{ n=3; q=0.1; r=0.1;

P: "a posteriori" state covariance

%number of state %std of process %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 h=@(x)x(1); s=[0;0;1]; % measurement equation % initial state % initial state with noise

x=s+q*randn(3,1); %initial state P = eye(n); N=20; xV = zeros(n,N); sV = zeros(n,N); zV = zeros(1,N);

% initial state covraiance % total dynamic steps %estmate %actual % allocate memory

for k=1:N z = h(s) + r*randn; sV(:,k)= s; zV(k) = z; [x, P] = ukf(f,x,P,h,z,Q,R); xV(:,k) = x; s = f(s) + q*randn(3,1); end for k=1:3 subplot(3,1,k) plot(1:N, sV(k,:), '-', 1:N, xV(k,:), '--') end %} % % By Yi Cao at Cranfield University, 04/01/2008 % L=numel(x); m=numel(z); alpha=1e-3; ki=0; beta=2; lambda=alpha^2*(L+ki)-L; c=L+lambda; %numer of states %numer of measurements %default, tunable %default, tunable %default, tunable %scaling factor %scaling factor %weights for means % plot results % measurments % save actual state % save measurment % ekf % save estimate % update process

Wm=[lambda/c 0.5/c+zeros(1,2*L)]; Wc=Wm; Wc(1)=Wc(1)+(1-alpha^2+beta); c=sqrt(c); X=sigmas(x,P,c);

%weights for covariance

%sigma points around x %unscented transformation of process %sigma points around x1 %deviation of X1 %unscented transformation of

[x1,X1,P1,X2]=ut(fstate,X,Wm,Wc,L,Q); % X1=sigmas(x1,P1,c); % X2=X1-x1(:,ones(1,size(X1,2))); [z1,Z1,P2,Z2]=ut(hmeas,X1,Wm,Wc,m,R); measurments P12=X2*diag(Wc)*Z2'; K=P12*inv(P2);

%transformed cross-covariance

x=x1+K*(z-z1); P=P1-K*P12';

%state update %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



X: Sigma points

A = c*chol(P)'; Y = x(:,ones(1,numel(x))); X = [x Y+A Y-A];

网站首页 | 网站地图
All rights reserved Powered by 大学生考试网 9299.net