root/applications/dual/SIDP/SIDPS/motor_sidps/update.m @ 1249

function [H_new C_new ]=update(H_old, C_old,index)
H_new=zeros(size(H_old)+[0 1]);
C_new=zeros(size(C_old)+[0 1]);
for i=1:index
    H_new(i)=H_old(i);
    C_new(i)=C_old(i);
end

for i=index+1:size(H_old,2)
    H_new(i+1)=H_old(i);
    C_new(i+1)=C_old(i);
end

H_new(index+1)=(H_old(index)+H_old(index+1))/2;
C_new(index+1)=(C_old(index)+C_old(index+1))/2;

end