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

조회 수: 2 (최근 30일)
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 CenterFile Exchange에서 Systems of Nonlinear Equations에 대해 자세히 알아보기

Community Treasure Hunt

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

Start Hunting!

Translated by