// File: 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
subq $0x280, %rsp            # imm = 0x280
movq %rdi, -0x8(%rbp)
movsd %xmm0, -0x10(%rbp)
movq -0x8(%rbp), %rax
movq %rax, -0xf8(%rbp)
testb $0x1, 0x208(%rax)
jne 0x816c
movq -0xf8(%rbp), %rdi
movb $0x1, 0x208(%rdi)
addq $0x258, %rdi            # imm = 0x258
xorl %esi, %esi
callq 0x1fd40
movq -0xf8(%rbp), %rdi
movsd (%rax), %xmm0
movsd %xmm0, -0x180(%rbp)
addq $0x270, %rdi            # imm = 0x270
xorl %esi, %esi
callq 0x1fd40
movsd -0x180(%rbp), %xmm0
movq -0xf8(%rbp), %rdi
movsd %xmm0, (%rax)
addq $0x270, %rdi            # imm = 0x270
movl $0x1, %esi
callq 0x1fd40
movq -0xf8(%rbp), %rdi
xorps %xmm0, %xmm0
movsd %xmm0, (%rax)
addq $0x258, %rdi            # imm = 0x258
movl $0x1, %esi
callq 0x1fd40
movq -0xf8(%rbp), %rdi
movsd (%rax), %xmm0
movsd %xmm0, -0x178(%rbp)
addq $0x270, %rdi            # imm = 0x270
movl $0x2, %esi
callq 0x1fd40
movsd -0x178(%rbp), %xmm0
movq -0xf8(%rbp), %rdi
movsd %xmm0, (%rax)
addq $0x258, %rdi            # imm = 0x258
movl $0x2, %esi
callq 0x1fd40
movq -0xf8(%rbp), %rdi
movsd (%rax), %xmm0
movsd %xmm0, -0x170(%rbp)
addq $0x270, %rdi            # imm = 0x270
movl $0x3, %esi
callq 0x1fd40
movsd -0x170(%rbp), %xmm0
movq -0xf8(%rbp), %rdi
movsd %xmm0, (%rax)
addq $0x270, %rdi            # imm = 0x270
movl $0x4, %esi
callq 0x1fd40
movq -0xf8(%rbp), %rdi
xorps %xmm0, %xmm0
movsd %xmm0, (%rax)
addq $0x270, %rdi            # imm = 0x270
movl $0x5, %esi
callq 0x1fd40
movq -0xf8(%rbp), %rdi
xorps %xmm0, %xmm0
movsd %xmm0, (%rax)
addq $0x270, %rdi            # imm = 0x270
movl $0x2, %esi
callq 0x1fd40
movsd (%rax), %xmm0
callq 0x4450
movq -0xf8(%rbp), %rax
movsd %xmm0, -0x28(%rbp)
movsd 0x1b8(%rax), %xmm0
callq 0x4450
movq -0xf8(%rbp), %rdi
movsd %xmm0, -0x168(%rbp)
addq $0x2a0, %rdi            # imm = 0x2A0
xorl %esi, %esi
callq 0x1fe60
movsd -0x168(%rbp), %xmm0
movq %rax, %rcx
movq -0xf8(%rbp), %rax
movsd %xmm0, (%rcx)
movsd 0x2b8(%rax), %xmm0
callq 0x4450
movq -0xf8(%rbp), %rdi
movsd %xmm0, -0x160(%rbp)
addq $0x258, %rdi            # imm = 0x258
movl $0x2, %esi
callq 0x1fd40
movsd -0x160(%rbp), %xmm0
mulsd (%rax), %xmm0
callq 0x4450
movq -0xf8(%rbp), %rdi
movsd %xmm0, -0x158(%rbp)
addq $0x2a0, %rdi            # imm = 0x2A0
movl $0x1, %esi
callq 0x1fe60
movsd -0x158(%rbp), %xmm0
movq %rax, %rcx
movq -0xf8(%rbp), %rax
movsd %xmm0, 0x8(%rcx)
movsd 0x2d8(%rax), %xmm0
mulsd -0x28(%rbp), %xmm0
callq 0x4450
movq -0xf8(%rbp), %rdi
movsd %xmm0, -0x150(%rbp)
addq $0x2a0, %rdi            # imm = 0x2A0
movl $0x2, %esi
callq 0x1fe60
movsd -0x150(%rbp), %xmm0
movq %rax, %rcx
movq -0xf8(%rbp), %rax
movsd %xmm0, 0x10(%rcx)
movsd 0x2b8(%rax), %xmm0
callq 0x4450
movq -0xf8(%rbp), %rdi
movsd %xmm0, -0x148(%rbp)
addq $0x258, %rdi            # imm = 0x258
movl $0x1, %esi
callq 0x1fd40
movsd -0x148(%rbp), %xmm0
mulsd (%rax), %xmm0
callq 0x4450
movq -0xf8(%rbp), %rax
movsd %xmm0, -0x140(%rbp)
movsd 0x250(%rax), %xmm0
mulsd 0x2d8(%rax), %xmm0
mulsd -0x28(%rbp), %xmm0
callq 0x4450
movq -0xf8(%rbp), %rdi
movaps %xmm0, %xmm1
movsd -0x140(%rbp), %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, -0x138(%rbp)
addq $0x2a0, %rdi            # imm = 0x2A0
movl $0x3, %esi
callq 0x1fe60
movsd -0x138(%rbp), %xmm0
movq -0xf8(%rbp), %rdi
movsd %xmm0, 0x18(%rax)
movsd 0x2d8(%rdi), %xmm0
movsd %xmm0, -0x130(%rbp)
addq $0x258, %rdi            # imm = 0x258
movl $0x1, %esi
callq 0x1fd40
movsd -0x130(%rbp), %xmm0
mulsd (%rax), %xmm0
callq 0x4450
movq -0xf8(%rbp), %rdi
movsd %xmm0, -0x128(%rbp)
addq $0x258, %rdi            # imm = 0x258
movl $0x1, %esi
callq 0x1fd40
movsd -0x128(%rbp), %xmm0
movq -0xf8(%rbp), %rdi
mulsd (%rax), %xmm0
movsd %xmm0, -0x120(%rbp)
addq $0x258, %rdi            # imm = 0x258
movl $0x2, %esi
callq 0x1fd40
movsd -0x120(%rbp), %xmm0
movq -0xf8(%rbp), %rdi
mulsd (%rax), %xmm0
movsd %xmm0, -0x118(%rbp)
addq $0x2a0, %rdi            # imm = 0x2A0
movl $0x2, %esi
callq 0x1fe60
movsd -0x118(%rbp), %xmm0
movq -0xf8(%rbp), %rdi
movsd %xmm0, 0x18(%rax)
addq $0x2a0, %rdi            # imm = 0x2A0
movl $0x2, %esi
callq 0x1fe60
movq -0xf8(%rbp), %rdi
movsd 0x18(%rax), %xmm0
movsd %xmm0, -0x110(%rbp)
addq $0x2a0, %rdi            # imm = 0x2A0
movl $0x3, %esi
callq 0x1fe60
movsd -0x110(%rbp), %xmm0
movq -0xf8(%rbp), %rdi
movsd %xmm0, 0x10(%rax)
movsd 0x108(%rdi), %xmm0
mulsd -0x28(%rbp), %xmm0
movsd %xmm0, -0x108(%rbp)
addq $0x2a0, %rdi            # imm = 0x2A0
movl $0x4, %esi
callq 0x1fe60
movsd -0x108(%rbp), %xmm0
movq -0xf8(%rbp), %rdi
movsd %xmm0, 0x20(%rax)
addq $0x2a0, %rdi            # imm = 0x2A0
movl $0x4, %esi
callq 0x1fe60
movq -0xf8(%rbp), %rdi
movsd 0x20(%rax), %xmm0
movsd %xmm0, -0x100(%rbp)
addq $0x2a0, %rdi            # imm = 0x2A0
movl $0x5, %esi
callq 0x1fe60
movsd -0x100(%rbp), %xmm0
movsd %xmm0, 0x28(%rax)
movq -0xf8(%rbp), %rdi
movq %rdi, %rsi
addq $0x270, %rsi            # imm = 0x270
movq %rsi, -0x1b8(%rbp)
movq %rdi, %rdx
addq $0x110, %rdx            # imm = 0x110
movq %rdx, -0x268(%rbp)
callq 0xa3b0
movq -0x268(%rbp), %rsi
movq -0xf8(%rbp), %rdi
movsd -0x10(%rbp), %xmm0
callq 0xa650
movq -0x268(%rbp), %rsi
movq -0x1b8(%rbp), %rdx
movq -0xf8(%rbp), %rdi
callq 0xaaf0
movq -0xf8(%rbp), %rsi
addq $0x288, %rsi            # imm = 0x288
movq %rsi, -0x190(%rbp)
leaq -0x50(%rbp), %rdi
movq %rdi, -0x260(%rbp)
xorps %xmm0, %xmm0
callq 0x1ffd0
movq -0x260(%rbp), %rdi
callq 0x1f580
movq -0x190(%rbp), %rdi
xorl %esi, %esi
movl %esi, -0x254(%rbp)
callq 0x1fe60
movl -0x254(%rbp), %esi
movq -0x190(%rbp), %rdi
movabsq $0x3ff0000000000000, %rcx # imm = 0x3FF0000000000000
movq %rcx, (%rax)
movsd -0x10(%rbp), %xmm0
movsd %xmm0, -0x250(%rbp)
callq 0x1fe60
movsd -0x250(%rbp), %xmm0
movq -0x1b8(%rbp), %rdi
movsd %xmm0, 0x8(%rax)
movl $0x3, %esi
movl %esi, -0x1f4(%rbp)
callq 0x1fd40
movq -0x190(%rbp), %rdi
movsd (%rax), %xmm1
addsd %xmm1, %xmm1
movsd -0x10(%rbp), %xmm0
mulsd %xmm0, %xmm1
movsd 0x1bd84(%rip), %xmm0    # 0x24008
movsd %xmm0, -0x1a0(%rbp)
subsd %xmm1, %xmm0
movsd %xmm0, -0x248(%rbp)
movl $0x1, %esi
movl %esi, -0x21c(%rbp)
callq 0x1fe60
movsd -0x248(%rbp), %xmm0
movl -0x21c(%rbp), %esi
movq -0x1b8(%rbp), %rdi
movsd %xmm0, 0x8(%rax)
callq 0x1fd40
movl -0x21c(%rbp), %esi
movq -0x190(%rbp), %rdi
movsd (%rax), %xmm0
movsd 0x1bf48(%rip), %xmm1    # 0x24228
mulsd %xmm1, %xmm0
movsd -0x10(%rbp), %xmm1
mulsd %xmm1, %xmm0
movsd %xmm0, -0x240(%rbp)
callq 0x1fe60
movsd -0x240(%rbp), %xmm0
movl -0x21c(%rbp), %esi
movq -0x190(%rbp), %rdi
movsd %xmm0, 0x18(%rax)
movsd -0x10(%rbp), %xmm0
movsd %xmm0, -0x238(%rbp)
callq 0x1fe60
movsd -0x238(%rbp), %xmm0
movl -0x1f4(%rbp), %esi
movq -0x1b8(%rbp), %rdi
movsd %xmm0, 0x28(%rax)
callq 0x1fd40
movsd -0x1a0(%rbp), %xmm0
movq -0x190(%rbp), %rdi
movsd (%rax), %xmm1
movsd -0x10(%rbp), %xmm2
mulsd %xmm2, %xmm1
subsd %xmm1, %xmm0
movsd %xmm0, -0x230(%rbp)
movl $0x2, %esi
movl %esi, -0x1b0(%rbp)
callq 0x1fe60
movsd -0x230(%rbp), %xmm0
movq -0x1b8(%rbp), %rdi
movl -0x1b0(%rbp), %esi
movsd %xmm0, 0x10(%rax)
callq 0x1fd40
movl -0x1b0(%rbp), %esi
movq -0x190(%rbp), %rdi
movsd (%rax), %xmm0
movapd 0x1bdab(%rip), %xmm1    # 0x24160
movaps %xmm1, -0x1e0(%rbp)
pxor %xmm1, %xmm0
movsd -0x10(%rbp), %xmm1
mulsd %xmm1, %xmm0
movsd %xmm0, -0x228(%rbp)
callq 0x1fe60
movsd -0x228(%rbp), %xmm0
movl -0x21c(%rbp), %esi
movq -0x1b8(%rbp), %rdi
movsd %xmm0, 0x18(%rax)
callq 0x1fd40
movl -0x1f4(%rbp), %esi
movq -0x190(%rbp), %rdi
movsd (%rax), %xmm0
addsd %xmm0, %xmm0
movsd -0x10(%rbp), %xmm1
mulsd %xmm1, %xmm0
movsd %xmm0, -0x218(%rbp)
callq 0x1fe60
movsd -0x218(%rbp), %xmm0
movl -0x1f4(%rbp), %esi
movq -0x1b8(%rbp), %rdi
movsd %xmm0, 0x8(%rax)
callq 0x1fd40
movl -0x1f4(%rbp), %esi
movsd -0x1a0(%rbp), %xmm0
movq -0x190(%rbp), %rdi
movsd (%rax), %xmm1
addsd %xmm1, %xmm1
movsd -0x10(%rbp), %xmm2
mulsd %xmm2, %xmm1
subsd %xmm1, %xmm0
movsd %xmm0, -0x210(%rbp)
callq 0x1fe60
movsd -0x210(%rbp), %xmm0
movl -0x1f4(%rbp), %esi
movq -0x190(%rbp), %rdi
movsd %xmm0, 0x18(%rax)
movsd -0x10(%rbp), %xmm0
movsd %xmm0, -0x208(%rbp)
callq 0x1fe60
movsd -0x208(%rbp), %xmm0
movq -0x1b8(%rbp), %rdi
movsd %xmm0, 0x20(%rax)
movl $0x4, %esi
movl %esi, -0x1ac(%rbp)
callq 0x1fd40
movaps -0x1e0(%rbp), %xmm1
movl -0x1ac(%rbp), %esi
movq -0x190(%rbp), %rdi
movsd (%rax), %xmm0
pxor %xmm1, %xmm0
movsd -0x10(%rbp), %xmm1
mulsd %xmm1, %xmm0
movsd %xmm0, -0x200(%rbp)
callq 0x1fe60
movsd -0x200(%rbp), %xmm1
movl -0x1f4(%rbp), %esi
movq -0x1b8(%rbp), %rdi
movsd -0x1a0(%rbp), %xmm0
movq %rax, %rcx
movq -0xf8(%rbp), %rax
movsd %xmm1, 0x18(%rcx)
movsd 0x100(%rax), %xmm1
divsd %xmm1, %xmm0
movsd %xmm0, -0x1f0(%rbp)
callq 0x1fd40
movsd -0x1f0(%rbp), %xmm1
movl -0x1ac(%rbp), %esi
movsd -0x1a0(%rbp), %xmm0
movq -0x190(%rbp), %rdi
movsd (%rax), %xmm2
addsd %xmm2, %xmm1
movsd -0x10(%rbp), %xmm2
mulsd %xmm2, %xmm1
subsd %xmm1, %xmm0
movsd %xmm0, -0x1e8(%rbp)
callq 0x1fe60
movsd -0x1e8(%rbp), %xmm0
movq -0x1b8(%rbp), %rdi
movsd %xmm0, 0x20(%rax)
movl $0x5, %esi
movl %esi, -0x1c4(%rbp)
callq 0x1fd40
movaps -0x1e0(%rbp), %xmm1
movl -0x1c4(%rbp), %esi
movq -0x190(%rbp), %rdi
movsd (%rax), %xmm0
pxor %xmm1, %xmm0
movsd -0x10(%rbp), %xmm1
mulsd %xmm1, %xmm0
movsd %xmm0, -0x1d0(%rbp)
callq 0x1fe60
movsd -0x1d0(%rbp), %xmm0
movl -0x1ac(%rbp), %esi
movq -0x190(%rbp), %rdi
movsd %xmm0, 0x18(%rax)
callq 0x1fe60
movl -0x1c4(%rbp), %esi
movq -0x190(%rbp), %rdi
movsd 0x20(%rax), %xmm0
movsd %xmm0, -0x1c0(%rbp)
callq 0x1fe60
movsd -0x1c0(%rbp), %xmm0
movq -0x1b8(%rbp), %rdi
movl -0x1b0(%rbp), %esi
movsd %xmm0, 0x28(%rax)
callq 0x1fd40
movsd (%rax), %xmm0
callq 0x4450
movl -0x1ac(%rbp), %esi
movq -0xf8(%rbp), %rax
movq -0x190(%rbp), %rdi
addsd %xmm0, %xmm0
movsd -0x10(%rbp), %xmm1
mulsd %xmm1, %xmm0
movsd 0x100(%rax), %xmm1
movsd 0x1b9b7(%rip), %xmm2    # 0x24020
mulsd %xmm2, %xmm1
divsd %xmm1, %xmm0
movsd %xmm0, -0x20(%rbp)
movsd -0x20(%rbp), %xmm0
movsd 0x108(%rax), %xmm1
mulsd %xmm1, %xmm0
movsd %xmm0, -0x18(%rbp)
movsd -0x18(%rbp), %xmm0
movsd %xmm0, -0x198(%rbp)
callq 0x1fe60
movsd 0x20(%rax), %xmm0
callq 0x4450
movl -0x1ac(%rbp), %esi
movq -0x190(%rbp), %rdi
movsd %xmm0, -0x1a8(%rbp)
callq 0x1fe60
movsd -0x1a8(%rbp), %xmm1
movsd -0x1a0(%rbp), %xmm2
movsd -0x198(%rbp), %xmm0
movq -0xf8(%rbp), %rsi
movsd 0x20(%rax), %xmm3
addsd %xmm3, %xmm1
addsd %xmm2, %xmm1
mulsd %xmm1, %xmm0
movsd %xmm0, -0x30(%rbp)
movsd -0x30(%rbp), %xmm0
movsd %xmm0, -0x38(%rbp)
addq $0x2a0, %rsi            # imm = 0x2A0
leaq -0xb0(%rbp), %rdi
movq %rdi, -0x188(%rbp)
callq 0x1f3e0
movq -0x190(%rbp), %rsi
movq -0x188(%rbp), %rdx
leaq -0x98(%rbp), %rdi
callq 0x203c0
jmp 0x8737
movq -0xf8(%rbp), %rsi
addq $0x288, %rsi            # imm = 0x288
leaq -0xf0(%rbp), %rdi
callq 0x1f3e0
jmp 0x8753
leaq -0xd8(%rbp), %rdi
leaq -0xf0(%rbp), %rsi
callq 0x23320
jmp 0x8768
leaq -0x80(%rbp), %rdi
leaq -0x98(%rbp), %rsi
leaq -0xd8(%rbp), %rdx
callq 0x203c0
jmp 0x8781
movq -0xf8(%rbp), %rsi
addq $0x2a0, %rsi            # imm = 0x2A0
leaq -0x68(%rbp), %rdi
leaq -0x80(%rbp), %rdx
callq 0x1ff00
jmp 0x879e
leaq -0x68(%rbp), %rdi
callq 0x1f580
leaq -0x80(%rbp), %rdi
callq 0x1f580
leaq -0xd8(%rbp), %rdi
callq 0x1f580
leaq -0xf0(%rbp), %rdi
callq 0x1f580
leaq -0x98(%rbp), %rdi
callq 0x1f580
leaq -0xb0(%rbp), %rdi
callq 0x1f580
movq -0xf8(%rbp), %rdi
movsd -0x30(%rbp), %xmm0
movsd %xmm0, -0x278(%rbp)
addq $0x2a0, %rdi            # imm = 0x2A0
movl $0x4, %esi
callq 0x1fe60
movsd -0x278(%rbp), %xmm0
movq -0xf8(%rbp), %rdi
addsd 0x20(%rax), %xmm0
movsd %xmm0, 0x20(%rax)
movsd -0x38(%rbp), %xmm0
movsd %xmm0, -0x270(%rbp)
addq $0x2a0, %rdi            # imm = 0x2A0
movl $0x5, %esi
callq 0x1fe60
movsd -0x270(%rbp), %xmm0
addsd 0x28(%rax), %xmm0
movsd %xmm0, 0x28(%rax)
addq $0x280, %rsp            # imm = 0x280
popq %rbp
retq
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0xb8(%rbp)
movl %eax, -0xbc(%rbp)
jmp 0x88e6
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0xb8(%rbp)
movl %eax, -0xbc(%rbp)
jmp 0x88da
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0xb8(%rbp)
movl %eax, -0xbc(%rbp)
jmp 0x88ce
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0xb8(%rbp)
movl %eax, -0xbc(%rbp)
jmp 0x88c2
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0xb8(%rbp)
movl %eax, -0xbc(%rbp)
leaq -0x80(%rbp), %rdi
callq 0x1f580
leaq -0xd8(%rbp), %rdi
callq 0x1f580
leaq -0xf0(%rbp), %rdi
callq 0x1f580
leaq -0x98(%rbp), %rdi
callq 0x1f580
leaq -0xb0(%rbp), %rdi
callq 0x1f580
movq -0xb8(%rbp), %rdi
callq 0x33d0
nop