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

1 view (last 30 days)
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.

Answers (0)

Community Treasure Hunt

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

Start Hunting!