How to apply adaptive algorithm using k=block.CurrentTime in the s-function level2?

2 ビュー (過去 30 日間)
dab483
dab483 2014 年 4 月 14 日
Here is my s-function level2 algorithm for EKF
function Update(block)
Q = block.DialogPrm(2).Data; ??? this Q will be adapt later if k>M
R = block.DialogPrm(3).Data;
D = block.InputPort(2).Data;
W = block.InputPort(3).Data;
meas = block.InputPort(1).Data;
xhat = block.Dwork(1).Data;
P=reshape(block.Dwork(2).Data,4,4);
k=block.CurrentTime; ??????
%%1. Find the Jacobian
.....
%%2. Propagate the covariance matrix: (Time update)
P = A*P*A' + Q; ####to apply adaptive method for Q
%%3. Propagate the estimate:(Time update)
xhat(1) = ...
xhat(2) = ...
xhat(3) = ...
xhat(4) = ...
xhat-=[xhat(1);xhat(2);xhat(3);xhat(4)];
%%4 a). Compute observation estimates:(Measurement Update)
H = [1 0 0 0;0 1 0 0;0 0 1 0];
yhat = H*xhat;
%%4 c). Compute residual (Measurement Update)
residual = meas - yhat;
%%5. Compute Kalman Gain:(Measurement Update)
K = P*H'/(H*P*H'+ R);
%%6. Update estimate (Measurement Update)
xhat = xhat- + K*residual;
%%7. Update Covariance Matrix (Measurement Update)
P = (eye(4)-K*H)*P*(eye(4)-K*H)' + K*R*K';
block.Dwork(1).Data = xhat;
block.Dwork(2).Data = P(:);
---------------------------------------------------- Supposed i want to apply adaptive method for Q where at time k, the algorithm is defined as below:
ep(:,k)=meas(:,k)-H*xhat(:,k);
eta(:,k)=meas(:,k)-H*xhat-(:,k);
w(:,k)=K*(meas(:,k) - yhat);
Is(:,2*k)=ep(:,k);
Is(:,2*k-1)=eta(:,k);
if k>M
for i=1:4
DVQ=sum(w(i,k-M:k).^2)/(M);
end
Q=diag(DVQ)
end
------------------------------------------------- My question. how do i integrate the adaptive algorithm defined above into my s-function level2 (update)? in the s-function i have defined k=block.CurrentTime. Any advise? Thank you.

回答 (0 件)

カテゴリ

Help Center および File ExchangeSystems of Nonlinear Equations についてさらに検索

Community Treasure Hunt

Find the treasures in MATLAB Central and discover how the community can help you!

Start Hunting!

Translated by