// File: DavidLBruce[P]sim3dof/build/../src/FcTseO.cpp
// Function: FcTseO::PropStates(double)
void FcTseO::PropStates( double timeStep )
{
  double timeStep2 = timeStep * timeStep;
  double timeStep3 = timeStep2 * timeStep;

  double x2 = stateVec(0) * stateVec(0);

  phi[0][0] = 1.0;
  phi[0][1] = timeStep;
  phi[0][2] = 0.0;

  phi[1][0] = -stateVec(2) * timeStep;
  phi[1][1] = 1.0;
  phi[1][2] = -stateVec(0) * timeStep;

  phi[2][0] = 0.0;
  phi[2][1] = 0.0;
  phi[2][2] = 1.0;

  QQ[0][0] = 0.0;
  QQ[0][1] = 0.0;
  QQ[0][2] = 0.0;

  QQ[1][0] = 0.0;
  QQ[1][1] = x2 * timeStep3 * procNoise / 3.0;
  QQ[1][2] = -0.5 * stateVec(0) * timeStep2 * procNoise;

  QQ[2][0] = 0.0;
  QQ[2][1] = -0.5 * stateVec(0) * timeStep2 * procNoise;
  QQ[2][2] = timeStep * procNoise;


  //
  // Propagate the state vector
  //
  stateVec = IntegrateRK2( stateVec, timeStep );

  //
  // Propagate the estimation covariance matrix
  //
  PP = phi * PP * Transpose(phi) + QQ;

}

// Assembly:
pushq %rbp
movq %rsp, %rbp
subq $0x1e0, %rsp            # imm = 0x1E0
movq %rdi, -0x8(%rbp)
movsd %xmm0, -0x10(%rbp)
movq -0x8(%rbp), %rdi
movq %rdi, -0x140(%rbp)
movsd -0x10(%rbp), %xmm0
mulsd %xmm0, %xmm0
movsd %xmm0, -0x18(%rbp)
movsd -0x18(%rbp), %xmm0
movsd -0x10(%rbp), %xmm1
mulsd %xmm1, %xmm0
movsd %xmm0, -0x20(%rbp)
addq $0x248, %rdi            # imm = 0x248
movq %rdi, -0x138(%rbp)
xorl %esi, %esi
movl %esi, -0x16c(%rbp)
callq 0x1fd40
movl -0x16c(%rbp), %esi
movq -0x138(%rbp), %rdi
movsd (%rax), %xmm0
movsd %xmm0, -0x1c8(%rbp)
callq 0x1fd40
movsd -0x1c8(%rbp), %xmm0
movl -0x16c(%rbp), %esi
movq -0x140(%rbp), %rdi
movsd (%rax), %xmm1
mulsd %xmm1, %xmm0
movsd %xmm0, -0x28(%rbp)
addq $0x1e0, %rdi            # imm = 0x1E0
movq %rdi, -0x198(%rbp)
callq 0x1fe60
movq -0x198(%rbp), %rdi
movl -0x16c(%rbp), %esi
movabsq $0x3ff0000000000000, %rcx # imm = 0x3FF0000000000000
movq %rcx, -0x190(%rbp)
movq %rcx, (%rax)
movsd -0x10(%rbp), %xmm0
movsd %xmm0, -0x1c0(%rbp)
callq 0x1fe60
movsd -0x1c0(%rbp), %xmm0
movq -0x198(%rbp), %rdi
movl -0x16c(%rbp), %esi
movsd %xmm0, 0x8(%rax)
callq 0x1fe60
movq -0x138(%rbp), %rdi
movq $0x0, 0x10(%rax)
movl $0x2, %esi
movl %esi, -0x14c(%rbp)
callq 0x1fd40
movq -0x198(%rbp), %rdi
movsd (%rax), %xmm0
movapd 0x1f1ce(%rip), %xmm1    # 0x24160
movaps %xmm1, -0x1b0(%rbp)
pxor %xmm1, %xmm0
movsd -0x10(%rbp), %xmm1
mulsd %xmm1, %xmm0
movsd %xmm0, -0x1b8(%rbp)
movl $0x1, %esi
movl %esi, -0x17c(%rbp)
callq 0x1fe60
movsd -0x1b8(%rbp), %xmm0
movq -0x198(%rbp), %rdi
movl -0x17c(%rbp), %esi
movsd %xmm0, (%rax)
callq 0x1fe60
movq -0x190(%rbp), %rcx
movl -0x16c(%rbp), %esi
movq -0x138(%rbp), %rdi
movq %rcx, 0x8(%rax)
callq 0x1fd40
movaps -0x1b0(%rbp), %xmm1
movq -0x198(%rbp), %rdi
movl -0x17c(%rbp), %esi
movsd (%rax), %xmm0
pxor %xmm1, %xmm0
movsd -0x10(%rbp), %xmm1
mulsd %xmm1, %xmm0
movsd %xmm0, -0x1a0(%rbp)
callq 0x1fe60
movsd -0x1a0(%rbp), %xmm0
movq -0x198(%rbp), %rdi
movl -0x14c(%rbp), %esi
movsd %xmm0, 0x10(%rax)
callq 0x1fe60
movq -0x198(%rbp), %rdi
movl -0x14c(%rbp), %esi
movq $0x0, (%rax)
callq 0x1fe60
movq -0x198(%rbp), %rdi
movl -0x14c(%rbp), %esi
movq $0x0, 0x8(%rax)
callq 0x1fe60
movq -0x190(%rbp), %rcx
movl -0x16c(%rbp), %esi
movq -0x140(%rbp), %rdi
movq %rcx, 0x10(%rax)
addq $0x228, %rdi            # imm = 0x228
movq %rdi, -0x158(%rbp)
callq 0x1fe60
movl -0x16c(%rbp), %esi
movq -0x158(%rbp), %rdi
movq $0x0, (%rax)
callq 0x1fe60
movl -0x16c(%rbp), %esi
movq -0x158(%rbp), %rdi
movq $0x0, 0x8(%rax)
callq 0x1fe60
movl -0x17c(%rbp), %esi
movq -0x158(%rbp), %rdi
movq $0x0, 0x10(%rax)
callq 0x1fe60
movl -0x17c(%rbp), %esi
movq -0x158(%rbp), %rdi
movq %rax, %rcx
movq -0x140(%rbp), %rax
movq $0x0, (%rcx)
movsd -0x28(%rbp), %xmm0
movsd -0x20(%rbp), %xmm1
mulsd %xmm1, %xmm0
movsd 0x130(%rax), %xmm1
mulsd %xmm1, %xmm0
movsd 0x1eeeb(%rip), %xmm1    # 0x24020
divsd %xmm1, %xmm0
movsd %xmm0, -0x188(%rbp)
callq 0x1fe60
movsd -0x188(%rbp), %xmm0
movl -0x16c(%rbp), %esi
movq -0x138(%rbp), %rdi
movsd %xmm0, 0x8(%rax)
callq 0x1fd40
movl -0x17c(%rbp), %esi
movq -0x158(%rbp), %rdi
movq %rax, %rcx
movq -0x140(%rbp), %rax
movsd (%rcx), %xmm0
movsd 0x1efc8(%rip), %xmm1    # 0x24150
movsd %xmm1, -0x168(%rbp)
mulsd %xmm1, %xmm0
movsd -0x18(%rbp), %xmm1
mulsd %xmm1, %xmm0
movsd 0x130(%rax), %xmm1
mulsd %xmm1, %xmm0
movsd %xmm0, -0x178(%rbp)
callq 0x1fe60
movsd -0x178(%rbp), %xmm0
movq -0x158(%rbp), %rdi
movl -0x14c(%rbp), %esi
movsd %xmm0, 0x10(%rax)
callq 0x1fe60
movl -0x16c(%rbp), %esi
movq -0x138(%rbp), %rdi
movq $0x0, (%rax)
callq 0x1fd40
movsd -0x168(%rbp), %xmm1
movq -0x158(%rbp), %rdi
movl -0x14c(%rbp), %esi
movq %rax, %rcx
movq -0x140(%rbp), %rax
movsd (%rcx), %xmm0
mulsd %xmm1, %xmm0
movsd -0x18(%rbp), %xmm1
mulsd %xmm1, %xmm0
movsd 0x130(%rax), %xmm1
mulsd %xmm1, %xmm0
movsd %xmm0, -0x160(%rbp)
callq 0x1fe60
movsd -0x160(%rbp), %xmm0
movq -0x158(%rbp), %rdi
movl -0x14c(%rbp), %esi
movq %rax, %rcx
movq -0x140(%rbp), %rax
movsd %xmm0, 0x8(%rcx)
movsd -0x10(%rbp), %xmm0
movsd 0x130(%rax), %xmm1
mulsd %xmm1, %xmm0
movsd %xmm0, -0x148(%rbp)
callq 0x1fe60
movsd -0x148(%rbp), %xmm0
movq -0x140(%rbp), %rsi
movq -0x138(%rbp), %rdx
movsd %xmm0, 0x10(%rax)
movsd -0x10(%rbp), %xmm0
leaq -0x58(%rbp), %rdi
movq %rdi, -0x130(%rbp)
callq 0x6640
movq -0x138(%rbp), %rsi
movq -0x130(%rbp), %rdx
leaq -0x40(%rbp), %rdi
callq 0x1ff00
jmp 0x52c2
leaq -0x40(%rbp), %rdi
callq 0x1f580
leaq -0x58(%rbp), %rdi
callq 0x1f580
movq -0x140(%rbp), %rsi
movq %rsi, %rax
addq $0x1e0, %rax            # imm = 0x1E0
movq %rax, -0x1d8(%rbp)
addq $0x1f8, %rsi            # imm = 0x1F8
leaq -0xe0(%rbp), %rdi
movq %rdi, -0x1d0(%rbp)
callq 0x1f3e0
movq -0x1d8(%rbp), %rsi
movq -0x1d0(%rbp), %rdx
leaq -0xc8(%rbp), %rdi
callq 0x203c0
jmp 0x5321
movq -0x140(%rbp), %rsi
addq $0x1e0, %rsi            # imm = 0x1E0
leaq -0x110(%rbp), %rdi
callq 0x1f3e0
jmp 0x533d
leaq -0xf8(%rbp), %rdi
leaq -0x110(%rbp), %rsi
callq 0x23320
jmp 0x5352
leaq -0xb0(%rbp), %rdi
leaq -0xc8(%rbp), %rsi
leaq -0xf8(%rbp), %rdx
callq 0x203c0
jmp 0x536e
movq -0x140(%rbp), %rsi
addq $0x228, %rsi            # imm = 0x228
leaq -0x128(%rbp), %rdi
callq 0x1f3e0
jmp 0x538a
leaq -0x98(%rbp), %rdi
leaq -0xb0(%rbp), %rsi
leaq -0x128(%rbp), %rdx
callq 0x20020
jmp 0x53a6
movq -0x140(%rbp), %rsi
addq $0x1f8, %rsi            # imm = 0x1F8
leaq -0x80(%rbp), %rdi
leaq -0x98(%rbp), %rdx
callq 0x1ff00
jmp 0x53c6
leaq -0x80(%rbp), %rdi
callq 0x1f580
leaq -0x98(%rbp), %rdi
callq 0x1f580
leaq -0x128(%rbp), %rdi
callq 0x1f580
leaq -0xb0(%rbp), %rdi
callq 0x1f580
leaq -0xf8(%rbp), %rdi
callq 0x1f580
leaq -0x110(%rbp), %rdi
callq 0x1f580
leaq -0xc8(%rbp), %rdi
callq 0x1f580
leaq -0xe0(%rbp), %rdi
callq 0x1f580
addq $0x1e0, %rsp            # imm = 0x1E0
popq %rbp
retq
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0x60(%rbp)
movl %eax, -0x64(%rbp)
leaq -0x58(%rbp), %rdi
callq 0x1f580
jmp 0x5500
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0x60(%rbp)
movl %eax, -0x64(%rbp)
jmp 0x54f4
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0x60(%rbp)
movl %eax, -0x64(%rbp)
jmp 0x54e8
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0x60(%rbp)
movl %eax, -0x64(%rbp)
jmp 0x54dc
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0x60(%rbp)
movl %eax, -0x64(%rbp)
jmp 0x54d0
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0x60(%rbp)
movl %eax, -0x64(%rbp)
jmp 0x54c4
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0x60(%rbp)
movl %eax, -0x64(%rbp)
jmp 0x54b8
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0x60(%rbp)
movl %eax, -0x64(%rbp)
leaq -0x98(%rbp), %rdi
callq 0x1f580
leaq -0x128(%rbp), %rdi
callq 0x1f580
leaq -0xb0(%rbp), %rdi
callq 0x1f580
leaq -0xf8(%rbp), %rdi
callq 0x1f580
leaq -0x110(%rbp), %rdi
callq 0x1f580
leaq -0xc8(%rbp), %rdi
callq 0x1f580
leaq -0xe0(%rbp), %rdi
callq 0x1f580
movq -0x60(%rbp), %rdi
callq 0x33d0
nopl (%rax)