// File: /workspace/github_cpp_makefile_star2_O2/DavidLBruce[P]sim3dof/build/../src/FcTse6.cpp
// Function: FcTse6::PropStates(double)
void FcTse6::PropStates( double deltaTime )
{
  double alphaR;
  double cCoeff;
  double invRng2;
  double Q44;
  double Q55;

  if ( !initialized )
  {
    initialized = true;

    mscState(0) = measVec(0);    // LOS angle
    mscState(1) = 0.0;           // LOS rate
    mscState(2) = measVec(1);    // Inverse range
    mscState(3) = measVec(2);    // rDot / range
    mscState(4) = 0.0;           // Acceleration along LOS / range
    mscState(5) = 0.0;           // Acceleration across LOS / range

    invRng2 = SQ( mscState(2) );

    PP[0][0] = SQ( angSig );
    
    PP[1][1] = SQ( SQ(rDotSig) * measVec(2) );

    PP[2][2] = SQ( rngSig * invRng2 );

    PP[3][3] = SQ( SQ(rDotSig) * measVec(1) ) +
               SQ( measRdot * rngSig * invRng2 );

    PP[2][3] = SQ( rngSig * measVec(1) ) * measVec(1) * measVec(2);
    PP[3][2] = PP[2][3];

    PP[4][4] = singerVar * invRng2;
    PP[5][5] = PP[4][4];

  }  // end if ( !initialized )

  //
  // Propagate the state vector in cartesian coordinates
  //
  MscToCart( mscState,
             cartRel );


  IntegrateRK2( cartRel, deltaTime );

  CartToMsc( cartRel,
             mscState );

  //
  // Propagate the estimation covariance matrix
  //
  //
  // State transition Jacobian
  //
  phi = 0.0;

  phi[0][0] = 1.0;
  phi[0][1] = deltaTime;

  phi[1][1] = 1.0 - 2.0 * mscState(3) * deltaTime;
  phi[1][3] = -2.0 * mscState(1) * deltaTime;
  phi[1][5] = deltaTime;

  phi[2][2] = 1.0 - mscState(3) * deltaTime;
  phi[2][3] = -mscState(2) * deltaTime;

  phi[3][1] = 2.0 * mscState(1) * deltaTime;
  phi[3][3] = 1.0 - 2.0 * mscState(3) * deltaTime;
  phi[3][4] = deltaTime;

  phi[4][3] = -mscState(4) * deltaTime;
  phi[4][4] = 1.0 - (1.0 / singerTau + mscState(3)) * deltaTime;
 
  phi[5][3] = -mscState(5) * deltaTime;
  phi[5][5] = phi[4][4];

  //
  // Process noise matrix
  //
  cCoeff = 2.0 * SQ( mscState(2) ) * deltaTime / ( 3.0 * singerTau );

  alphaR = cCoeff * singerVar;
    
  Q44 = alphaR * ( SQ(phi[4][4]) + phi[4][4] + 1.0 );

  Q55 = Q44;

  //
  // Propagate the estimation covariance matrix.
  //
  PP = phi * PP * Transpose( phi );
  PP[4][4] += Q44;
  PP[5][5] += Q55;
 
}

// Assembly:
pushq %rbp
movq %rsp, %rbp
pushq %r15
pushq %r14
pushq %r13
pushq %r12
pushq %rbx
subq $0xf8, %rsp
movsd %xmm0, -0x48(%rbp)
movq %rdi, %rbx
cmpb $0x0, 0x208(%rdi)
jne 0x67d4
movb $0x1, 0x208(%rbx)
leaq 0x258(%rbx), %r14
movq %r14, %rdi
xorl %esi, %esi
callq 0x131c0
movsd (%rax), %xmm0
movsd %xmm0, -0x40(%rbp)
leaq 0x270(%rbx), %r12
movq %r12, %rdi
xorl %esi, %esi
callq 0x131c0
movsd -0x40(%rbp), %xmm0
movsd %xmm0, (%rax)
pushq $0x1
popq %r13
movq %r12, %rdi
movl %r13d, %esi
callq 0x131c0
andq $0x0, (%rax)
movq %r14, %rdi
movl %r13d, %esi
callq 0x131c0
movsd (%rax), %xmm0
movsd %xmm0, -0x40(%rbp)
pushq $0x2
popq %r15
movq %r12, %rdi
movl %r15d, %esi
callq 0x131c0
movsd -0x40(%rbp), %xmm0
movsd %xmm0, (%rax)
movq %r14, %rdi
movl %r15d, %esi
callq 0x131c0
movsd (%rax), %xmm0
movsd %xmm0, -0x40(%rbp)
pushq $0x3
popq %rsi
movq %r12, %rdi
callq 0x131c0
movsd -0x40(%rbp), %xmm0
movsd %xmm0, (%rax)
pushq $0x4
popq %rsi
movq %r12, %rdi
callq 0x131c0
andq $0x0, (%rax)
pushq $0x5
popq %rsi
movq %r12, %rdi
callq 0x131c0
andq $0x0, (%rax)
movq %r12, %rdi
movl %r15d, %esi
callq 0x131c0
movsd (%rax), %xmm0
mulsd %xmm0, %xmm0
movsd %xmm0, -0x60(%rbp)
movsd 0x1b8(%rbx), %xmm0
mulsd %xmm0, %xmm0
movsd %xmm0, -0x40(%rbp)
leaq 0x2a0(%rbx), %r12
movq %r12, %rdi
xorl %esi, %esi
callq 0x1324e
movsd -0x40(%rbp), %xmm0
movsd %xmm0, (%rax)
movsd 0x2b8(%rbx), %xmm0
mulsd %xmm0, %xmm0
movsd %xmm0, -0x40(%rbp)
movq %r14, %rdi
movl %r15d, %esi
callq 0x131c0
movsd -0x40(%rbp), %xmm0
mulsd (%rax), %xmm0
mulsd %xmm0, %xmm0
movsd %xmm0, -0x40(%rbp)
movq %r12, %rdi
movl %r13d, %esi
callq 0x1324e
movsd -0x40(%rbp), %xmm0
movsd %xmm0, 0x8(%rax)
movsd 0x2d8(%rbx), %xmm0
mulsd -0x60(%rbp), %xmm0
mulsd %xmm0, %xmm0
movsd %xmm0, -0x40(%rbp)
movq %r12, %rdi
movl %r15d, %esi
callq 0x1324e
movsd -0x40(%rbp), %xmm0
movsd %xmm0, 0x10(%rax)
movsd 0x2b8(%rbx), %xmm0
mulsd %xmm0, %xmm0
movsd %xmm0, -0x40(%rbp)
movq %r14, %rdi
movl %r13d, %esi
callq 0x131c0
movsd -0x40(%rbp), %xmm0
mulsd (%rax), %xmm0
movsd 0x250(%rbx), %xmm1
mulsd 0x2d8(%rbx), %xmm1
mulsd %xmm0, %xmm0
mulsd -0x60(%rbp), %xmm1
mulsd %xmm1, %xmm1
addsd %xmm0, %xmm1
movsd %xmm1, -0x40(%rbp)
movq %r12, %rdi
pushq $0x3
popq %rsi
callq 0x1324e
movsd -0x40(%rbp), %xmm0
movsd %xmm0, 0x18(%rax)
movsd 0x2d8(%rbx), %xmm0
movsd %xmm0, -0x40(%rbp)
movq %r14, %rdi
movl %r13d, %esi
callq 0x131c0
movsd -0x40(%rbp), %xmm0
mulsd (%rax), %xmm0
mulsd %xmm0, %xmm0
movsd %xmm0, -0x40(%rbp)
movq %r14, %rdi
movl %r13d, %esi
callq 0x131c0
movsd -0x40(%rbp), %xmm0
mulsd (%rax), %xmm0
movsd %xmm0, -0x40(%rbp)
movq %r14, %rdi
movl %r15d, %esi
callq 0x131c0
movsd -0x40(%rbp), %xmm0
mulsd (%rax), %xmm0
movsd %xmm0, -0x40(%rbp)
movq %r12, %rdi
movl %r15d, %esi
callq 0x1324e
movsd -0x40(%rbp), %xmm0
movsd %xmm0, 0x18(%rax)
movq %r12, %rdi
movl %r15d, %esi
callq 0x1324e
movsd 0x18(%rax), %xmm0
movsd %xmm0, -0x40(%rbp)
movq %r12, %rdi
pushq $0x3
popq %rsi
callq 0x1324e
movsd -0x40(%rbp), %xmm0
movsd %xmm0, 0x10(%rax)
movsd -0x60(%rbp), %xmm0
mulsd 0x108(%rbx), %xmm0
movsd %xmm0, -0x60(%rbp)
movq %r12, %rdi
pushq $0x4
popq %r14
movl %r14d, %esi
callq 0x1324e
movsd -0x60(%rbp), %xmm0
movsd %xmm0, 0x20(%rax)
movq %r12, %rdi
movl %r14d, %esi
callq 0x1324e
movsd 0x20(%rax), %xmm0
movsd %xmm0, -0x40(%rbp)
movq %r12, %rdi
pushq $0x5
popq %rsi
callq 0x1324e
movsd -0x40(%rbp), %xmm0
movsd %xmm0, 0x28(%rax)
leaq 0x270(%rbx), %r15
leaq 0x110(%rbx), %r14
movq %r15, %rsi
movq %r14, %rdx
callq 0x7e3c
movq %rbx, %rdi
movq %r14, %rsi
movsd -0x48(%rbp), %xmm0
callq 0x8042
movq %r14, %rsi
movq %r15, %rdx
callq 0x83aa
leaq 0x288(%rbx), %r14
leaq -0x120(%rbp), %r12
xorps %xmm0, %xmm0
movq %r12, %rdi
movq %r14, %rsi
callq 0x132f0
movq %r12, %rdi
callq 0x12dd8
movq %r14, %rdi
xorl %esi, %esi
callq 0x1324e
movabsq $0x3ff0000000000000, %rcx # imm = 0x3FF0000000000000
movq %rcx, (%rax)
movq %r14, %rdi
xorl %esi, %esi
callq 0x1324e
movsd -0x48(%rbp), %xmm0
movsd %xmm0, 0x8(%rax)
pushq $0x3
popq %r12
movq %r15, %rdi
movl %r12d, %esi
callq 0x131c0
movsd (%rax), %xmm1
mulsd 0xe9b6(%rip), %xmm1     # 0x15228
mulsd -0x48(%rbp), %xmm1
movsd 0xe7b9(%rip), %xmm0     # 0x15038
movsd %xmm0, -0x40(%rbp)
addsd %xmm0, %xmm1
movsd %xmm1, -0x60(%rbp)
pushq $0x1
popq %r13
movq %r14, %rdi
movl %r13d, %esi
callq 0x1324e
movsd -0x60(%rbp), %xmm0
movsd %xmm0, 0x8(%rax)
movq %r15, %rdi
movl %r13d, %esi
callq 0x131c0
movsd (%rax), %xmm0
mulsd 0xe96b(%rip), %xmm0     # 0x15228
mulsd -0x48(%rbp), %xmm0
movsd %xmm0, -0x60(%rbp)
movq %r14, %rdi
movl %r13d, %esi
callq 0x1324e
movsd -0x60(%rbp), %xmm0
movsd %xmm0, 0x18(%rax)
movq %r14, %rdi
movl %r13d, %esi
callq 0x1324e
movsd -0x48(%rbp), %xmm0
movsd %xmm0, 0x28(%rax)
movq %r15, %rdi
movl %r12d, %esi
callq 0x131c0
movsd (%rax), %xmm0
mulsd -0x48(%rbp), %xmm0
movsd 0xe72b(%rip), %xmm1     # 0x15038
subsd %xmm0, %xmm1
movsd %xmm1, -0x60(%rbp)
pushq $0x2
popq %r12
movq %r14, %rdi
movl %r12d, %esi
callq 0x1324e
movsd -0x60(%rbp), %xmm0
movsd %xmm0, 0x10(%rax)
movq %r15, %rdi
movl %r12d, %esi
callq 0x131c0
movsd (%rax), %xmm0
xorpd 0xe6ca(%rip), %xmm0     # 0x15010
mulsd -0x48(%rbp), %xmm0
movapd %xmm0, -0x60(%rbp)
movq %r14, %rdi
movl %r12d, %esi
callq 0x1324e
movaps -0x60(%rbp), %xmm0
movsd %xmm0, 0x18(%rax)
movq %r15, %rdi
movl %r13d, %esi
callq 0x131c0
movsd (%rax), %xmm0
addsd %xmm0, %xmm0
mulsd -0x48(%rbp), %xmm0
movsd %xmm0, -0x60(%rbp)
movq %r14, %rdi
pushq $0x3
popq %r12
movl %r12d, %esi
callq 0x1324e
movsd -0x60(%rbp), %xmm0
movsd %xmm0, 0x8(%rax)
movq %r15, %rdi
movl %r12d, %esi
callq 0x131c0
movsd 0xe87b(%rip), %xmm0     # 0x15228
mulsd (%rax), %xmm0
mulsd -0x48(%rbp), %xmm0
addsd 0xe67a(%rip), %xmm0     # 0x15038
movsd %xmm0, -0x60(%rbp)
movq %r14, %rdi
movl %r12d, %esi
callq 0x1324e
movsd -0x60(%rbp), %xmm0
movsd %xmm0, 0x18(%rax)
movq %r14, %rdi
movl %r12d, %esi
callq 0x1324e
movsd -0x48(%rbp), %xmm0
movsd %xmm0, 0x20(%rax)
pushq $0x4
popq %r13
movq %r15, %rdi
movl %r13d, %esi
callq 0x131c0
movsd (%rax), %xmm0
xorpd 0xe608(%rip), %xmm0     # 0x15010
mulsd -0x48(%rbp), %xmm0
movapd %xmm0, -0x60(%rbp)
movq %r14, %rdi
movl %r13d, %esi
callq 0x1324e
movaps -0x60(%rbp), %xmm0
movsd %xmm0, 0x18(%rax)
movsd 0xe60a(%rip), %xmm0     # 0x15038
divsd 0x100(%rbx), %xmm0
movsd %xmm0, -0x60(%rbp)
movq %r15, %rdi
movl %r12d, %esi
callq 0x131c0
movsd -0x60(%rbp), %xmm0
addsd (%rax), %xmm0
mulsd -0x48(%rbp), %xmm0
movapd %xmm0, %xmm1
movsd -0x40(%rbp), %xmm0
subsd %xmm1, %xmm0
movsd %xmm0, -0x40(%rbp)
movq %r14, %rdi
movl %r13d, %esi
callq 0x1324e
movsd -0x40(%rbp), %xmm0
movsd %xmm0, 0x20(%rax)
pushq $0x5
popq %r12
movq %r15, %rdi
movl %r12d, %esi
callq 0x131c0
movsd (%rax), %xmm0
xorpd 0xe57a(%rip), %xmm0     # 0x15010
mulsd -0x48(%rbp), %xmm0
movapd %xmm0, -0x40(%rbp)
movq %r14, %rdi
movl %r12d, %esi
callq 0x1324e
movaps -0x40(%rbp), %xmm0
movsd %xmm0, 0x18(%rax)
movq %r14, %rdi
movl %r13d, %esi
callq 0x1324e
movsd 0x20(%rax), %xmm0
movsd %xmm0, -0x40(%rbp)
movq %r14, %rdi
movl %r12d, %esi
callq 0x1324e
movsd -0x40(%rbp), %xmm0
movsd %xmm0, 0x28(%rax)
movq %r15, %rdi
pushq $0x2
popq %rsi
callq 0x131c0
movsd (%rax), %xmm0
movsd %xmm0, -0x78(%rbp)
movsd 0x100(%rbx), %xmm0
movsd %xmm0, -0x70(%rbp)
movsd 0x108(%rbx), %xmm0
movsd %xmm0, -0x60(%rbp)
movq %r14, %rdi
movl %r13d, %esi
callq 0x1324e
movsd 0x20(%rax), %xmm0
movsd %xmm0, -0x40(%rbp)
movq %r14, %rdi
movl %r13d, %esi
callq 0x1324e
movsd 0x20(%rax), %xmm0
movsd %xmm0, -0x68(%rbp)
addq $0x2a0, %rbx            # imm = 0x2A0
leaq -0xf0(%rbp), %r15
movq %r15, %rdi
movq %rbx, %rsi
callq 0x12d48
leaq -0xc0(%rbp), %rdi
movq %r14, %rsi
movq %r15, %rdx
callq 0x13512
leaq -0x90(%rbp), %rdi
movq %r14, %rsi
callq 0x12d48
leaq -0xa8(%rbp), %rdi
leaq -0x90(%rbp), %rsi
callq 0x14d68
leaq -0xd8(%rbp), %rdi
leaq -0xc0(%rbp), %rsi
leaq -0xa8(%rbp), %rdx
callq 0x13512
leaq -0x108(%rbp), %rdi
leaq -0xd8(%rbp), %rdx
movq %rbx, %rsi
callq 0x13296
movsd -0x40(%rbp), %xmm0
mulsd %xmm0, %xmm0
addsd -0x68(%rbp), %xmm0
movapd %xmm0, %xmm2
movsd -0x78(%rbp), %xmm1
mulsd %xmm1, %xmm1
addsd %xmm1, %xmm1
mulsd -0x48(%rbp), %xmm1
movsd -0x70(%rbp), %xmm0
mulsd 0xe43c(%rip), %xmm0     # 0x15020
divsd %xmm0, %xmm1
movsd -0x60(%rbp), %xmm0
mulsd %xmm1, %xmm0
addsd 0xe43f(%rip), %xmm2     # 0x15038
mulsd %xmm0, %xmm2
movsd %xmm2, -0x40(%rbp)
leaq -0x108(%rbp), %rdi
callq 0x12dd8
leaq -0xd8(%rbp), %rdi
callq 0x12dd8
leaq -0xa8(%rbp), %rdi
callq 0x12dd8
leaq -0x90(%rbp), %rdi
callq 0x12dd8
leaq -0xc0(%rbp), %rdi
callq 0x12dd8
leaq -0xf0(%rbp), %rdi
callq 0x12dd8
pushq $0x4
popq %rsi
movq %rbx, %rdi
callq 0x1324e
movsd 0x20(%rax), %xmm0
addsd -0x40(%rbp), %xmm0
movsd %xmm0, 0x20(%rax)
pushq $0x5
popq %rsi
movq %rbx, %rdi
callq 0x1324e
movsd -0x40(%rbp), %xmm0
addsd 0x28(%rax), %xmm0
movsd %xmm0, 0x28(%rax)
addq $0xf8, %rsp
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq
movq %rax, %rbx
leaq -0xd8(%rbp), %rdi
callq 0x12dd8
jmp 0x6ca4
movq %rax, %rbx
leaq -0xa8(%rbp), %rdi
callq 0x12dd8
jmp 0x6cb5
movq %rax, %rbx
leaq -0x90(%rbp), %rdi
callq 0x12dd8
jmp 0x6cc6
movq %rax, %rbx
leaq -0xc0(%rbp), %rdi
callq 0x12dd8
jmp 0x6cd7
movq %rax, %rbx
leaq -0xf0(%rbp), %rdi
callq 0x12dd8
movq %rbx, %rdi
callq 0x3320
nop