function egoIMU = EgoSimImu (egoVehiclePossX,egoVehiclePossY,clock)
%funkce pro vytvoření IMU hodnot ze známých v poloh v čase
    %% inicializace proměnných  
    persistent prevPoss;
    if isempty(prevPoss)
        prevPoss =  zeros(10,3);% [egoVehiclePossX egoVehiclePossY clock];
    end

    persistent prevVelo;
    if isempty(prevVelo)
        prevVelo =  zeros(10,1);
    end

    persistent prevAngleRot;
    if isempty(prevAngleRot)
        prevAngleRot =  zeros(10,1);
    end

    persistent wheelRot;
    if isempty(wheelRot)
        wheelRot =  zeros(10,1);
    end

    %zapsaní nové pozice na provní řádek + FIFO
    actualPoss = [egoVehiclePossX egoVehiclePossY clock];
    prevPoss = circshift(prevPoss, [1, 0]);
    prevPoss(1,:) = actualPoss;

    %rychlost 
    prevVelo = circshift(prevVelo, [1, 0]);
    prevVelo(1,1)= carCalcFunction.CalcVeloActual(prevPoss(1:2,1:2),prevPoss(:,3));
    
    % zrychlení  
    egoAcell = carCalcFunction.CalcAccActual(prevVelo(1:2,1),prevPoss(:,3));

    % orientace auta y+ Sever (0°) x+ východ (90°) 
    prevAngleRot = circshift(prevAngleRot, [1, 0]);
    [prevAngleRot(1,1), ~ , ~] = carCalcFunction.WorldRot(prevPoss(:,1:2),2);

    % změna uhlu směru + po směru hodin
    if (prevAngleRot(1,1)-prevAngleRot(2,1)) < 180
        alfa= prevAngleRot(1,1)-prevAngleRot(2,1);
    else
        alfa= prevAngleRot(1,1)-prevAngleRot(2,1)-360;
    end 

  %% získání bočního zrychlení

% zjednodužení že rychlost je rovnoběžná s vektorem posledních 2 bodů a radius je k nim kolmý
    radiusZataceni=abs(tand(90-alfa)*sqrt((prevPoss(1,1)-prevPoss(2,1))^2+(prevPoss(1,2)-prevPoss(2,2))^2));
    
    if alfa==0
        uhlovaRychlost=0;
    elseif sign(prevVelo(1,1))==-1
        uhlovaRychlost=-prevVelo(1,1)/radiusZataceni; %
    else
        uhlovaRychlost=prevVelo(1,1)/radiusZataceni; %
    end

    dt=prevPoss(1,3)-prevPoss(2,3);

    egoAccelX=egoAcell;
    if sign(alfa)==-1
        egoAccelY=uhlovaRychlost^2*radiusZataceni;
    else
        egoAccelY=-(uhlovaRychlost^2)*radiusZataceni;
    end
  
    omegaEgoZ=alfa/dt;

    %% zápis hodnot do výstupní matice

    egoIMU = zeros(1,4);
    
    egoIMU(1,1)=egoAccelX;
    egoIMU(1,2)=egoAccelY;
    egoIMU(1,3)=omegaEgoZ;
    egoIMU(1,4)=prevAngleRot(1,1); %magnetic Sever


end
