unscentedkfadd.m

惯性导航系统传递对准系统模型程序

function [xEst,PEst,inovation,S]=UnscentedKFADD(xEst,PEst,z,Q,R,ffun,hfun,modelpara,alpha,beta,kappa)

states       = size(xEst(:),1);
observations = size(z(:),1);
vNoise       = size(Q,2);
wNoise       = size(R,2);
noises       = vNoise+wNoise; 

PQ=PEst;
xQ=xEst;

[xSigmaPts, wSigmaPts, nsp] = scaledSymmetricSigmaPoints(xQ, PQ, alpha, beta, kappa); 
wSigmaPts_xmat = repmat(wSigmaPts(:,2:nsp),states,1);
wSigmaPts_zmat = repmat(wSigmaPts(:,2:nsp),observations,1);
xPredSigmaPts = feval(ffun,xSigmaPts(1:states,:),[],modelpara);
xPred = sum(wSigmaPts_xmat .* (xPredSigmaPts(:,2:nsp) - repmat(xPredSigmaPts(:,1),1,nsp-1)),2);
xPred=xPred+xPredSigmaPts(:,1);

exSigmaPt = xPredSigmaPts(:,1)-xPred;
PPred   = wSigmaPts(nsp+1)*exSigmaPt*exSigmaPt';
exSigmaPt = xPredSigmaPts(:,2:nsp) - repmat(xPred,1,nsp-1);
PPred   = PPred + (wSigmaPts_xmat .* exSigmaPt) * exSigmaPt';
PPred   = PPred + Q;

xPredSigmaPts = scaledSymmetricSigmaPoints(xPred, PPred, alpha, beta, kappa); 

zPredSigmaPts = feval(hfun,xPredSigmaPts,[],modelpara);
zPred = sum(wSigmaPts_zmat .* (zPredSigmaPts(:,2:nsp) - repmat(zPredSigmaPts(:,1),1,nsp-1)),2);
zPred=zPred+zPredSigmaPts(:,1);

exSigmaPt = xPredSigmaPts(:,1)-xPred;
ezSigmaPt = zPredSigmaPts(:,1)-zPred;
PxzPred = wSigmaPts(nsp+1)*exSigmaPt*ezSigmaPt';
S       = wSigmaPts(nsp+1)*ezSigmaPt*ezSigmaPt';

ezSigmaPt = zPredSigmaPts(:,2:nsp) - repmat(zPred,1,nsp-1);
exSigmaPt = xPredSigmaPts(:,2:nsp) - repmat(xPred,1,nsp-1);
S         = S + (wSigmaPts_zmat .* ezSigmaPt) * ezSigmaPt'+R;
PxzPred   = PxzPred + exSigmaPt * (wSigmaPts_zmat .* ezSigmaPt)';

K  = PxzPred / S;
inovation = z - zPred;
xEst = xPred + K*inovation;
PEst = PPred - K*S*K';
PEst=make_symmetric(PEst);