// File: /workspace/github_cpp_makefile_star2_O1/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 %r12
pushq %rbx
subq $0xf0, %rsp
movsd %xmm0, -0x38(%rbp)
movq %rdi, %rbx
cmpb $0x0, 0x208(%rdi)
jne 0x693c
movb $0x1, 0x208(%rbx)
leaq 0x258(%rbx), %r14
xorl %r12d, %r12d
movq %r14, %rdi
xorl %esi, %esi
callq 0x13aba
movsd (%rax), %xmm0
movsd %xmm0, -0x30(%rbp)
leaq 0x270(%rbx), %r15
movq %r15, %rdi
xorl %esi, %esi
callq 0x13aba
movsd -0x30(%rbp), %xmm0
movsd %xmm0, (%rax)
movq %r15, %rdi
movl $0x1, %esi
callq 0x13aba
movq %r12, (%rax)
movq %r14, %rdi
movl $0x1, %esi
callq 0x13aba
movsd (%rax), %xmm0
movsd %xmm0, -0x30(%rbp)
movq %r15, %rdi
movl $0x2, %esi
callq 0x13aba
movsd -0x30(%rbp), %xmm0
movsd %xmm0, (%rax)
movq %r14, %rdi
movl $0x2, %esi
callq 0x13aba
movsd (%rax), %xmm0
movsd %xmm0, -0x30(%rbp)
movq %r15, %rdi
movl $0x3, %esi
callq 0x13aba
movsd -0x30(%rbp), %xmm0
movsd %xmm0, (%rax)
movq %r15, %rdi
movl $0x4, %esi
callq 0x13aba
movq %r12, (%rax)
movq %r15, %rdi
movl $0x5, %esi
callq 0x13aba
movq %r12, (%rax)
movq %r15, %rdi
movl $0x2, %esi
callq 0x13aba
movsd (%rax), %xmm0
mulsd %xmm0, %xmm0
movsd %xmm0, -0x50(%rbp)
movsd 0x1b8(%rbx), %xmm0
mulsd %xmm0, %xmm0
movsd %xmm0, -0x30(%rbp)
leaq 0x2a0(%rbx), %r15
movq %r15, %rdi
xorl %esi, %esi
callq 0x13b48
movsd -0x30(%rbp), %xmm0
movsd %xmm0, (%rax)
movsd 0x2b8(%rbx), %xmm0
mulsd %xmm0, %xmm0
movsd %xmm0, -0x30(%rbp)
movq %r14, %rdi
movl $0x2, %esi
callq 0x13aba
movsd -0x30(%rbp), %xmm0
mulsd (%rax), %xmm0
mulsd %xmm0, %xmm0
movsd %xmm0, -0x30(%rbp)
movq %r15, %rdi
movl $0x1, %esi
callq 0x13b48
movsd -0x30(%rbp), %xmm0
movsd %xmm0, 0x8(%rax)
movsd 0x2d8(%rbx), %xmm0
mulsd -0x50(%rbp), %xmm0
mulsd %xmm0, %xmm0
movsd %xmm0, -0x30(%rbp)
movq %r15, %rdi
movl $0x2, %esi
callq 0x13b48
movsd -0x30(%rbp), %xmm0
movsd %xmm0, 0x10(%rax)
movsd 0x2b8(%rbx), %xmm0
mulsd %xmm0, %xmm0
movsd %xmm0, -0x30(%rbp)
movq %r14, %rdi
movl $0x1, %esi
callq 0x13aba
movsd -0x30(%rbp), %xmm0
mulsd (%rax), %xmm0
movsd 0x250(%rbx), %xmm1
mulsd 0x2d8(%rbx), %xmm1
mulsd %xmm0, %xmm0
mulsd -0x50(%rbp), %xmm1
mulsd %xmm1, %xmm1
addsd %xmm0, %xmm1
movsd %xmm1, -0x30(%rbp)
movq %r15, %rdi
movl $0x3, %esi
callq 0x13b48
movsd -0x30(%rbp), %xmm0
movsd %xmm0, 0x18(%rax)
movsd 0x2d8(%rbx), %xmm0
movsd %xmm0, -0x30(%rbp)
movq %r14, %rdi
movl $0x1, %esi
callq 0x13aba
movsd -0x30(%rbp), %xmm0
mulsd (%rax), %xmm0
mulsd %xmm0, %xmm0
movsd %xmm0, -0x30(%rbp)
movq %r14, %rdi
movl $0x1, %esi
callq 0x13aba
movsd -0x30(%rbp), %xmm0
mulsd (%rax), %xmm0
movsd %xmm0, -0x30(%rbp)
movq %r14, %rdi
movl $0x2, %esi
callq 0x13aba
movsd -0x30(%rbp), %xmm0
mulsd (%rax), %xmm0
movsd %xmm0, -0x30(%rbp)
movq %r15, %rdi
movl $0x2, %esi
callq 0x13b48
movsd -0x30(%rbp), %xmm0
movsd %xmm0, 0x18(%rax)
movq %r15, %rdi
movl $0x2, %esi
callq 0x13b48
movsd 0x18(%rax), %xmm0
movsd %xmm0, -0x30(%rbp)
movq %r15, %rdi
movl $0x3, %esi
callq 0x13b48
movsd -0x30(%rbp), %xmm0
movsd %xmm0, 0x10(%rax)
movsd -0x50(%rbp), %xmm0
mulsd 0x108(%rbx), %xmm0
movsd %xmm0, -0x50(%rbp)
movq %r15, %rdi
movl $0x4, %esi
callq 0x13b48
movsd -0x50(%rbp), %xmm0
movsd %xmm0, 0x20(%rax)
movq %r15, %rdi
movl $0x4, %esi
callq 0x13b48
movsd 0x20(%rax), %xmm0
movsd %xmm0, -0x30(%rbp)
movq %r15, %rdi
movl $0x5, %esi
callq 0x13b48
movsd -0x30(%rbp), %xmm0
movsd %xmm0, 0x28(%rax)
leaq 0x270(%rbx), %r15
leaq 0x110(%rbx), %r14
movq %r15, %rsi
movq %r14, %rdx
callq 0x7fe6
movq %rbx, %rdi
movq %r14, %rsi
movsd -0x38(%rbp), %xmm0
callq 0x81e4
movq %r14, %rsi
movq %r15, %rdx
callq 0x854c
leaq 0x288(%rbx), %r14
leaq -0x110(%rbp), %r12
xorps %xmm0, %xmm0
movq %r12, %rdi
movq %r14, %rsi
callq 0x13c1a
movq %r12, %rdi
callq 0x13642
movq %r14, %rdi
xorl %esi, %esi
callq 0x13b48
movabsq $0x3ff0000000000000, %rcx # imm = 0x3FF0000000000000
movq %rcx, (%rax)
movq %r14, %rdi
xorl %esi, %esi
callq 0x13b48
movsd -0x38(%rbp), %xmm0
movsd %xmm0, 0x8(%rax)
movq %r15, %rdi
movl $0x3, %esi
callq 0x13aba
movsd (%rax), %xmm1
mulsd 0xf850(%rip), %xmm1     # 0x16228
mulsd -0x38(%rbp), %xmm1
movsd 0xf653(%rip), %xmm0     # 0x16038
movsd %xmm0, -0x30(%rbp)
addsd %xmm0, %xmm1
movsd %xmm1, -0x50(%rbp)
movq %r14, %rdi
movl $0x1, %esi
callq 0x13b48
movsd -0x50(%rbp), %xmm0
movsd %xmm0, 0x8(%rax)
movq %r15, %rdi
movl $0x1, %esi
callq 0x13aba
movsd (%rax), %xmm0
mulsd 0xf805(%rip), %xmm0     # 0x16228
mulsd -0x38(%rbp), %xmm0
movsd %xmm0, -0x50(%rbp)
movq %r14, %rdi
movl $0x1, %esi
callq 0x13b48
movsd -0x50(%rbp), %xmm0
movsd %xmm0, 0x18(%rax)
movq %r14, %rdi
movl $0x1, %esi
callq 0x13b48
movsd -0x38(%rbp), %xmm0
movsd %xmm0, 0x28(%rax)
movq %r15, %rdi
movl $0x3, %esi
callq 0x13aba
movsd (%rax), %xmm0
mulsd -0x38(%rbp), %xmm0
movsd 0xf5bf(%rip), %xmm1     # 0x16038
subsd %xmm0, %xmm1
movsd %xmm1, -0x50(%rbp)
movq %r14, %rdi
movl $0x2, %esi
callq 0x13b48
movsd -0x50(%rbp), %xmm0
movsd %xmm0, 0x10(%rax)
movq %r15, %rdi
movl $0x2, %esi
callq 0x13aba
movsd (%rax), %xmm0
xorpd 0xf55e(%rip), %xmm0     # 0x16010
mulsd -0x38(%rbp), %xmm0
movapd %xmm0, -0x50(%rbp)
movq %r14, %rdi
movl $0x2, %esi
callq 0x13b48
movaps -0x50(%rbp), %xmm0
movsd %xmm0, 0x18(%rax)
movq %r15, %rdi
movl $0x1, %esi
callq 0x13aba
movsd (%rax), %xmm0
addsd %xmm0, %xmm0
mulsd -0x38(%rbp), %xmm0
movsd %xmm0, -0x50(%rbp)
movq %r14, %rdi
movl $0x3, %esi
callq 0x13b48
movsd -0x50(%rbp), %xmm0
movsd %xmm0, 0x8(%rax)
movq %r15, %rdi
movl $0x3, %esi
callq 0x13aba
movsd 0xf70b(%rip), %xmm0     # 0x16228
mulsd (%rax), %xmm0
mulsd -0x38(%rbp), %xmm0
addsd 0xf50a(%rip), %xmm0     # 0x16038
movsd %xmm0, -0x50(%rbp)
movq %r14, %rdi
movl $0x3, %esi
callq 0x13b48
movsd -0x50(%rbp), %xmm0
movsd %xmm0, 0x18(%rax)
movq %r14, %rdi
movl $0x3, %esi
callq 0x13b48
movsd -0x38(%rbp), %xmm0
movsd %xmm0, 0x20(%rax)
movq %r15, %rdi
movl $0x4, %esi
callq 0x13aba
movsd (%rax), %xmm0
xorpd 0xf496(%rip), %xmm0     # 0x16010
mulsd -0x38(%rbp), %xmm0
movapd %xmm0, -0x50(%rbp)
movq %r14, %rdi
movl $0x4, %esi
callq 0x13b48
movaps -0x50(%rbp), %xmm0
movsd %xmm0, 0x18(%rax)
movsd 0xf496(%rip), %xmm0     # 0x16038
divsd 0x100(%rbx), %xmm0
movsd %xmm0, -0x50(%rbp)
movq %r15, %rdi
movl $0x3, %esi
callq 0x13aba
movsd -0x50(%rbp), %xmm0
addsd (%rax), %xmm0
mulsd -0x38(%rbp), %xmm0
movapd %xmm0, %xmm1
movsd -0x30(%rbp), %xmm0
subsd %xmm1, %xmm0
movsd %xmm0, -0x30(%rbp)
movq %r14, %rdi
movl $0x4, %esi
callq 0x13b48
movsd -0x30(%rbp), %xmm0
movsd %xmm0, 0x20(%rax)
movq %r15, %rdi
movl $0x5, %esi
callq 0x13aba
movsd (%rax), %xmm0
xorpd 0xf404(%rip), %xmm0     # 0x16010
mulsd -0x38(%rbp), %xmm0
movapd %xmm0, -0x30(%rbp)
movq %r14, %rdi
movl $0x5, %esi
callq 0x13b48
movaps -0x30(%rbp), %xmm0
movsd %xmm0, 0x18(%rax)
movq %r14, %rdi
movl $0x4, %esi
callq 0x13b48
movsd 0x20(%rax), %xmm0
movsd %xmm0, -0x30(%rbp)
movq %r14, %rdi
movl $0x5, %esi
callq 0x13b48
movsd -0x30(%rbp), %xmm0
movsd %xmm0, 0x28(%rax)
movq %r15, %rdi
movl $0x2, %esi
callq 0x13aba
movsd (%rax), %xmm0
movsd %xmm0, -0x68(%rbp)
movsd 0x100(%rbx), %xmm0
movsd %xmm0, -0x60(%rbp)
movsd 0x108(%rbx), %xmm0
movsd %xmm0, -0x50(%rbp)
movq %r14, %rdi
movl $0x4, %esi
callq 0x13b48
movsd 0x20(%rax), %xmm0
movsd %xmm0, -0x30(%rbp)
movq %r14, %rdi
movl $0x4, %esi
callq 0x13b48
movsd 0x20(%rax), %xmm0
movsd %xmm0, -0x58(%rbp)
addq $0x2a0, %rbx            # imm = 0x2A0
leaq -0xe0(%rbp), %r15
movq %r15, %rdi
movq %rbx, %rsi
callq 0x13576
leaq -0xb0(%rbp), %rdi
movq %r14, %rsi
movq %r15, %rdx
callq 0x13e62
leaq -0x80(%rbp), %rdi
movq %r14, %rsi
callq 0x13576
leaq -0x98(%rbp), %rdi
leaq -0x80(%rbp), %rsi
callq 0x15940
leaq -0xc8(%rbp), %rdi
leaq -0xb0(%rbp), %rsi
leaq -0x98(%rbp), %rdx
callq 0x13e62
leaq -0xf8(%rbp), %rdi
leaq -0xc8(%rbp), %rdx
movq %rbx, %rsi
callq 0x13b92
movsd -0x30(%rbp), %xmm0
mulsd %xmm0, %xmm0
addsd -0x58(%rbp), %xmm0
movapd %xmm0, %xmm2
movsd -0x68(%rbp), %xmm1
mulsd %xmm1, %xmm1
addsd %xmm1, %xmm1
mulsd -0x38(%rbp), %xmm1
movsd -0x60(%rbp), %xmm0
mulsd 0xf2c0(%rip), %xmm0     # 0x16020
divsd %xmm0, %xmm1
movsd -0x50(%rbp), %xmm0
mulsd %xmm1, %xmm0
addsd 0xf2c3(%rip), %xmm2     # 0x16038
mulsd %xmm0, %xmm2
movsd %xmm2, -0x30(%rbp)
leaq -0xf8(%rbp), %rdi
callq 0x13642
leaq -0xc8(%rbp), %rdi
callq 0x13642
leaq -0x98(%rbp), %rdi
callq 0x13642
leaq -0x80(%rbp), %rdi
callq 0x13642
leaq -0xb0(%rbp), %rdi
callq 0x13642
leaq -0xe0(%rbp), %rdi
callq 0x13642
movq %rbx, %rdi
movl $0x4, %esi
callq 0x13b48
movsd 0x20(%rax), %xmm0
addsd -0x30(%rbp), %xmm0
movsd %xmm0, 0x20(%rax)
movq %rbx, %rdi
movl $0x5, %esi
callq 0x13b48
movsd -0x30(%rbp), %xmm0
addsd 0x28(%rax), %xmm0
movsd %xmm0, 0x28(%rax)
addq $0xf0, %rsp
popq %rbx
popq %r12
popq %r14
popq %r15
popq %rbp
retq
movq %rax, %rbx
leaq -0xc8(%rbp), %rdi
callq 0x13642
jmp 0x6e1f
movq %rax, %rbx
leaq -0x98(%rbp), %rdi
callq 0x13642
jmp 0x6e30
movq %rax, %rbx
leaq -0x80(%rbp), %rdi
callq 0x13642
jmp 0x6e3e
movq %rax, %rbx
leaq -0xb0(%rbp), %rdi
callq 0x13642
jmp 0x6e4f
movq %rax, %rbx
leaq -0xe0(%rbp), %rdi
callq 0x13642
movq %rbx, %rdi
callq 0x32d0
nop