Step 1

특정 시간에 대해서 C 벡터 함수의 해를 추정한다. 즉, 초기값을 설정한다. 이 추정치는 발산하지 않도록 잘 선택해야 한다.

Step 2

이 추정치를 기반으로 자코비안 행렬 과 C 벡터함수의 해를 구한다.

Δ q_{i} \to = - [C_{q_{i}}]^{- 1} C \to (q_{i} \to, t)

를 계산한다.

q_{i + 1} \to = q_{i} \to + Δ q_{i} \to

를 계산한다.

Step 2 ~ Step 5 를 사용자가 지정한 임계치보다 작을 때 까지 반복한다.

∣Δ q_{i} \to ∣ < ϵ_{1} OR ∣ C \to (q_{i} \to, t) ∣ < ϵ_{2}

속도 벡터를 구한다.

q \to \cdot = - [C_{q}]^{- 1} C_{t} \to

가속도 벡터 구하기

q \to \cdot\cdot = [C_{q}]^{- 1} Q_{d} \to Q_{d} \to = - (C_{q} q \to \cdot)_{q} q \to \cdot - 2 C_{qt} q \to \cdot - C_{tt} \to

Step 1 ~ Step 7 까지 다른 t를 설정한 뒤 반복한다.