AUKF自适应无迹卡尔曼滤波

function [x,P,C] = aukf(fstate, x, P, hmeas, z, Q, R,k1,C) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % 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 beta = 2; %default, tunable ki=0; u1=0; u2=1; nn=0.5; % 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 [y,Y1,P2,Y2] = ut(hmeas,X1,Wm,Wc,m,R); %unscented transformation of measurments sumc=0; % for j=1:L C(:,k1)=z-y; C_c=P2; d=C(:,k1)'*C_c^(-1)*C(:,k1); if d>=u2 M=1; elseif d<=u1 M=k1; else M=k1*nn^(d-u1); end for i=k1-M+1:k1 sumc=sumc+C(:,round(i))*C(:,round(i))'; end CK=1/M*sumc; ttt=trace(CK)/trace(C_c); b=max(1,abs(trace(CK)/trace(C_c))); % 滤波部分 P12 = X2*diag(Wc)*Y2'; %transformed cross-covariance K = (1/b)*P12*inv(P2); x = x1+K*(z-y); %state update P = P1-K*P12'; %covariance update