// File: /workspace/github_cpp_makefile_star2_O2/DavidLBruce[P]sim3dof/build/../src/FcTse6.cpp
// Function: FcTse6::UpdateStates()
void FcTse6::UpdateStates()
{
  Matrix   invTemp33(3,3);
  double   sigmaInvRng;
  double   sigmaInvTgo2;
  Matrix   temp0(3,3);
  Matrix   temp1(6,6);
  bool     test;

  //
  // Measurement covariance matrix
  //
  sigmaInvRng = rngSig * SQ(measVec(1));

  sigmaInvTgo2 = ( SQ(rDotSig) + SQ(rngSig * measVec(2)) ) * SQ( measVec(1) );

  RR[0][0] = SQ( angSig );
  RR[1][1] = SQ( sigmaInvRng );
  RR[2][2] = sigmaInvTgo2;
  RR[1][2] = SQ( rngSig ) * measVec(2) * SQ( measVec(1) ) * measVec(1);
  RR[2][1] = RR[1][2];

  //
  // Calculate the Kalman gains.
  //
  //  K = P * H' * [ H * P * H' + R]^-1
  //
  temp0 = HH * PP * HT + RR;  

//  test = temp0.Invert();

  test = inv33( temp0, invTemp33 );
  if ( !test )
  {
    cout << "FcTse::UpdateStates() temp33 matrix inversion fail @ time = " 
             << simTime << endl;
    exit( 1 );
  }
  temp0 = invTemp33;


  KK = PP * HT  * temp0;

  //
  // Update the state with the measurement
  //
  // residual = Z - H * x
  //
  residual = measVec - HH * mscState;

  innovation = KK * residual;

  mscState += innovation;

  //
  // Update the estimation covariance
  //
  temp1 = eye - KK * HH;

  PP = temp1 * PP * Transpose( temp1 ) + KK * RR * Transpose( KK );

}

// Assembly:
pushq %rbp
movq %rsp, %rbp
pushq %r15
pushq %r14
pushq %r13
pushq %r12
pushq %rbx
subq $0x3c8, %rsp            # imm = 0x3C8
movq %rdi, %r14
leaq -0xd8(%rbp), %rdi
pushq $0x3
popq %rbx
xorpd %xmm0, %xmm0
movl %ebx, %esi
movl %ebx, %edx
callq 0x12cf8
leaq -0x90(%rbp), %rdi
xorpd %xmm0, %xmm0
movl %ebx, %esi
movl %ebx, %edx
callq 0x12cf8
leaq -0xc0(%rbp), %rdi
pushq $0x6
popq %rdx
xorpd %xmm0, %xmm0
movl %edx, %esi
callq 0x12cf8
movsd 0x2d8(%r14), %xmm0
movsd %xmm0, -0x30(%rbp)
leaq 0x258(%r14), %r12
pushq $0x1
popq %rsi
movq %r12, %rdi
callq 0x131c0
movsd (%rax), %xmm0
movsd %xmm0, -0x38(%rbp)
movsd 0x2b8(%r14), %xmm0
movsd %xmm0, -0x40(%rbp)
movsd 0x2d8(%r14), %xmm0
movsd %xmm0, -0x48(%rbp)
pushq $0x2
popq %rsi
movq %r12, %rdi
callq 0x131c0
movsd (%rax), %xmm0
movsd %xmm0, -0xa0(%rbp)
pushq $0x1
popq %rsi
movq %r12, %rdi
callq 0x131c0
movsd (%rax), %xmm0
movsd %xmm0, -0x98(%rbp)
movsd 0x1b8(%r14), %xmm0
movsd %xmm0, -0xa8(%rbp)
leaq 0x2e0(%r14), %rbx
movq %rbx, %rdi
xorl %esi, %esi
callq 0x1324e
movsd -0xa8(%rbp), %xmm0
mulsd %xmm0, %xmm0
movsd %xmm0, (%rax)
pushq $0x1
popq %rsi
movq %rbx, %rdi
callq 0x1324e
movsd -0x38(%rbp), %xmm1
mulsd %xmm1, %xmm1
movsd -0x30(%rbp), %xmm0
mulsd %xmm1, %xmm0
mulsd %xmm0, %xmm0
movsd %xmm0, 0x8(%rax)
pushq $0x2
popq %rsi
movq %rbx, %rdi
callq 0x1324e
movsd -0x40(%rbp), %xmm1
mulsd %xmm1, %xmm1
movsd -0x48(%rbp), %xmm0
mulsd -0xa0(%rbp), %xmm0
mulsd %xmm0, %xmm0
addsd %xmm1, %xmm0
movsd -0x98(%rbp), %xmm1
mulsd %xmm1, %xmm1
mulsd %xmm0, %xmm1
movsd %xmm1, 0x10(%rax)
movsd 0x2d8(%r14), %xmm0
movsd %xmm0, -0x30(%rbp)
pushq $0x2
popq %rsi
movq %r12, %rdi
callq 0x131c0
movsd (%rax), %xmm0
movsd %xmm0, -0x38(%rbp)
pushq $0x1
popq %rsi
movq %r12, %rdi
callq 0x131c0
movsd (%rax), %xmm0
movsd %xmm0, -0x48(%rbp)
pushq $0x1
popq %rsi
movq %r12, %rdi
callq 0x131c0
movsd (%rax), %xmm0
movsd %xmm0, -0x40(%rbp)
pushq $0x1
popq %rsi
movq %rbx, %rdi
callq 0x1324e
movsd -0x30(%rbp), %xmm0
mulsd %xmm0, %xmm0
mulsd -0x38(%rbp), %xmm0
movsd -0x48(%rbp), %xmm1
mulsd %xmm1, %xmm1
mulsd %xmm0, %xmm1
mulsd -0x40(%rbp), %xmm1
movsd %xmm1, 0x10(%rax)
pushq $0x1
popq %rsi
movq %rbx, %rdi
callq 0x1324e
movsd 0x10(%rax), %xmm0
movsd %xmm0, -0x30(%rbp)
pushq $0x2
popq %rsi
movq %rbx, %rdi
callq 0x1324e
movsd -0x30(%rbp), %xmm0
movsd %xmm0, 0x8(%rax)
leaq 0x2a0(%r14), %rsi
leaq -0x330(%rbp), %rdi
movq %rsi, -0x30(%rbp)
callq 0x12d48
leaq 0x1d8(%r14), %rsi
leaq -0x78(%rbp), %rdi
leaq -0x330(%rbp), %rdx
movq %rsi, -0x38(%rbp)
callq 0x13512
leaq 0x1f0(%r14), %r15
leaq -0x318(%rbp), %rdi
movq %r15, %rsi
callq 0x12d48
leaq -0x60(%rbp), %rdi
leaq -0x78(%rbp), %rsi
leaq -0x318(%rbp), %rdx
callq 0x13512
leaq -0x300(%rbp), %rdi
movq %rbx, %rsi
callq 0x12d48
leaq -0x348(%rbp), %rdi
leaq -0x60(%rbp), %rsi
leaq -0x300(%rbp), %rdx
callq 0x1331e
leaq -0x3f0(%rbp), %rdi
leaq -0x90(%rbp), %rsi
leaq -0x348(%rbp), %rdx
callq 0x13296
leaq -0x3f0(%rbp), %rdi
callq 0x12dd8
leaq -0x348(%rbp), %rdi
callq 0x12dd8
leaq -0x300(%rbp), %rdi
callq 0x12dd8
leaq -0x60(%rbp), %rdi
callq 0x12dd8
leaq -0x318(%rbp), %rdi
callq 0x12dd8
leaq -0x78(%rbp), %rdi
callq 0x12dd8
leaq -0x330(%rbp), %rdi
callq 0x12dd8
leaq -0x90(%rbp), %rdi
leaq -0xd8(%rbp), %rsi
callq 0x866c
leaq -0x2e8(%rbp), %rdi
leaq -0xd8(%rbp), %rsi
callq 0x12d48
leaq -0x3d8(%rbp), %rdi
leaq -0x90(%rbp), %rsi
leaq -0x2e8(%rbp), %rdx
callq 0x13296
leaq -0x3d8(%rbp), %rdi
callq 0x12dd8
leaq -0x2e8(%rbp), %rdi
callq 0x12dd8
leaq -0x2b8(%rbp), %rdi
movq %r15, %rsi
callq 0x12d48
leaq -0x60(%rbp), %rdi
leaq -0x2b8(%rbp), %rdx
movq -0x30(%rbp), %rsi
callq 0x13512
leaq -0x2a0(%rbp), %rdi
leaq -0x90(%rbp), %rsi
callq 0x12d48
leaq -0x2d0(%rbp), %rdi
leaq -0x60(%rbp), %rsi
leaq -0x2a0(%rbp), %rdx
callq 0x13512
leaq 0x228(%r14), %r15
leaq -0x3c0(%rbp), %rdi
leaq -0x2d0(%rbp), %rdx
movq %r15, %rsi
callq 0x13296
leaq -0x3c0(%rbp), %rdi
callq 0x12dd8
leaq -0x2d0(%rbp), %rdi
callq 0x12dd8
leaq -0x2a0(%rbp), %rdi
callq 0x12dd8
leaq -0x60(%rbp), %rdi
callq 0x12dd8
leaq -0x2b8(%rbp), %rdi
callq 0x12dd8
leaq 0x270(%r14), %r13
leaq -0x258(%rbp), %rdi
movq %r13, %rsi
callq 0x12d48
leaq -0x270(%rbp), %rdi
leaq -0x258(%rbp), %rdx
movq -0x38(%rbp), %rsi
callq 0x13512
leaq -0x288(%rbp), %rdi
leaq -0x270(%rbp), %rdx
movq %r12, %rsi
callq 0x133aa
leaq 0x2c0(%r14), %r12
leaq -0x3a8(%rbp), %rdi
leaq -0x288(%rbp), %rdx
movq %r12, %rsi
callq 0x13296
leaq -0x3a8(%rbp), %rdi
callq 0x12dd8
leaq -0x288(%rbp), %rdi
callq 0x12dd8
leaq -0x270(%rbp), %rdi
callq 0x12dd8
leaq -0x258(%rbp), %rdi
callq 0x12dd8
leaq -0x228(%rbp), %rdi
movq %r12, %rsi
callq 0x12d48
leaq -0x240(%rbp), %rdi
leaq -0x228(%rbp), %rdx
movq %r15, %rsi
callq 0x13512
leaq 0x210(%r14), %r12
leaq -0x390(%rbp), %rdi
leaq -0x240(%rbp), %rdx
movq %r12, %rsi
callq 0x13296
leaq -0x390(%rbp), %rdi
callq 0x12dd8
leaq -0x240(%rbp), %rdi
callq 0x12dd8
leaq -0x228(%rbp), %rdi
callq 0x12dd8
leaq -0x210(%rbp), %rdi
movq %r12, %rsi
callq 0x12d48
leaq -0x210(%rbp), %rsi
movq %r13, %rdi
callq 0x137b0
leaq -0x210(%rbp), %rdi
callq 0x12dd8
leaq -0x1c8(%rbp), %rdi
movq -0x38(%rbp), %rsi
callq 0x12d48
leaq -0x1e0(%rbp), %rdi
leaq -0x1c8(%rbp), %rdx
movq %r15, %rsi
callq 0x13512
addq $0x1c0, %r14            # imm = 0x1C0
leaq -0x1f8(%rbp), %rdi
leaq -0x1e0(%rbp), %rdx
movq %r14, %rsi
callq 0x133aa
leaq -0x378(%rbp), %rdi
leaq -0xc0(%rbp), %rsi
leaq -0x1f8(%rbp), %rdx
callq 0x13296
leaq -0x378(%rbp), %rdi
callq 0x12dd8
leaq -0x1f8(%rbp), %rdi
callq 0x12dd8
leaq -0x1e0(%rbp), %rdi
callq 0x12dd8
leaq -0x1c8(%rbp), %rdi
callq 0x12dd8
leaq -0x198(%rbp), %rdi
movq -0x30(%rbp), %rsi
callq 0x12d48
leaq -0x78(%rbp), %rdi
leaq -0xc0(%rbp), %rsi
leaq -0x198(%rbp), %rdx
callq 0x13512
leaq -0x168(%rbp), %rdi
leaq -0xc0(%rbp), %rsi
callq 0x12d48
leaq -0x180(%rbp), %rdi
leaq -0x168(%rbp), %rsi
callq 0x14d68
leaq -0x60(%rbp), %rdi
leaq -0x78(%rbp), %rsi
leaq -0x180(%rbp), %rdx
callq 0x13512
leaq -0x120(%rbp), %rdi
movq %rbx, %rsi
callq 0x12d48
leaq -0x138(%rbp), %rdi
leaq -0x120(%rbp), %rdx
movq %r15, %rsi
callq 0x13512
leaq -0xf0(%rbp), %rdi
movq %r15, %rsi
callq 0x12d48
leaq -0x108(%rbp), %rdi
leaq -0xf0(%rbp), %rsi
callq 0x14d68
leaq -0x150(%rbp), %rdi
leaq -0x138(%rbp), %rsi
leaq -0x108(%rbp), %rdx
callq 0x13512
leaq -0x1b0(%rbp), %rdi
leaq -0x60(%rbp), %rsi
leaq -0x150(%rbp), %rdx
callq 0x1331e
leaq -0x360(%rbp), %rdi
leaq -0x1b0(%rbp), %rdx
movq -0x30(%rbp), %rsi
callq 0x13296
leaq -0x360(%rbp), %rdi
callq 0x12dd8
leaq -0x1b0(%rbp), %rdi
callq 0x12dd8
leaq -0x150(%rbp), %rdi
callq 0x12dd8
leaq -0x108(%rbp), %rdi
callq 0x12dd8
leaq -0xf0(%rbp), %rdi
callq 0x12dd8
leaq -0x138(%rbp), %rdi
callq 0x12dd8
leaq -0x120(%rbp), %rdi
callq 0x12dd8
leaq -0x60(%rbp), %rdi
callq 0x12dd8
leaq -0x180(%rbp), %rdi
callq 0x12dd8
leaq -0x168(%rbp), %rdi
callq 0x12dd8
leaq -0x78(%rbp), %rdi
callq 0x12dd8
leaq -0x198(%rbp), %rdi
callq 0x12dd8
leaq -0xc0(%rbp), %rdi
callq 0x12dd8
leaq -0x90(%rbp), %rdi
callq 0x12dd8
leaq -0xd8(%rbp), %rdi
callq 0x12dd8
addq $0x3c8, %rsp            # imm = 0x3C8
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq
movq %rax, %rbx
leaq -0x1b0(%rbp), %rdi
callq 0x12dd8
jmp 0x7446
movq %rax, %rbx
leaq -0x150(%rbp), %rdi
callq 0x12dd8
jmp 0x7457
movq %rax, %rbx
leaq -0x108(%rbp), %rdi
callq 0x12dd8
jmp 0x7468
movq %rax, %rbx
leaq -0xf0(%rbp), %rdi
callq 0x12dd8
jmp 0x7479
movq %rax, %rbx
leaq -0x138(%rbp), %rdi
callq 0x12dd8
leaq -0x120(%rbp), %rdi
callq 0x12dd8
leaq -0x60(%rbp), %rdi
callq 0x12dd8
leaq -0x180(%rbp), %rdi
callq 0x12dd8
leaq -0x168(%rbp), %rdi
callq 0x12dd8
leaq -0x78(%rbp), %rdi
callq 0x12dd8
leaq -0x198(%rbp), %rdi
jmp 0x7619
movq %rax, %rbx
jmp 0x7485
movq %rax, %rbx
jmp 0x7491
movq %rax, %rbx
jmp 0x749a
movq %rax, %rbx
jmp 0x74a6
movq %rax, %rbx
jmp 0x74b2
movq %rax, %rbx
jmp 0x74bb
jmp 0x7631
movq %rax, %rbx
leaq -0x1f8(%rbp), %rdi
callq 0x12dd8
jmp 0x74fe
movq %rax, %rbx
leaq -0x1e0(%rbp), %rdi
callq 0x12dd8
jmp 0x750f
movq %rax, %rbx
leaq -0x1c8(%rbp), %rdi
jmp 0x7619
movq %rax, %rbx
leaq -0x210(%rbp), %rdi
jmp 0x7619
movq %rax, %rbx
leaq -0x240(%rbp), %rdi
callq 0x12dd8
jmp 0x753e
movq %rax, %rbx
leaq -0x228(%rbp), %rdi
jmp 0x7619
movq %rax, %rbx
leaq -0x288(%rbp), %rdi
callq 0x12dd8
jmp 0x755e
movq %rax, %rbx
leaq -0x270(%rbp), %rdi
callq 0x12dd8
jmp 0x756f
movq %rax, %rbx
leaq -0x258(%rbp), %rdi
jmp 0x7619
movq %rax, %rbx
leaq -0x2d0(%rbp), %rdi
callq 0x12dd8
jmp 0x758f
movq %rax, %rbx
leaq -0x2a0(%rbp), %rdi
callq 0x12dd8
jmp 0x75a0
movq %rax, %rbx
leaq -0x60(%rbp), %rdi
callq 0x12dd8
jmp 0x75ae
movq %rax, %rbx
leaq -0x2b8(%rbp), %rdi
jmp 0x7619
jmp 0x7631
movq %rax, %rbx
leaq -0x2e8(%rbp), %rdi
jmp 0x7619
movq %rax, %rbx
leaq -0x348(%rbp), %rdi
callq 0x12dd8
jmp 0x75d9
movq %rax, %rbx
leaq -0x300(%rbp), %rdi
callq 0x12dd8
jmp 0x75ea
movq %rax, %rbx
leaq -0x60(%rbp), %rdi
callq 0x12dd8
jmp 0x75f8
movq %rax, %rbx
leaq -0x318(%rbp), %rdi
callq 0x12dd8
jmp 0x7609
movq %rax, %rbx
leaq -0x78(%rbp), %rdi
callq 0x12dd8
leaq -0x330(%rbp), %rdi
callq 0x12dd8
jmp 0x7634
movq %rax, %rbx
jmp 0x7612
jmp 0x7631
movq %rax, %rbx
jmp 0x7640
movq %rax, %rbx
jmp 0x764c
movq %rax, %rbx
leaq -0xc0(%rbp), %rdi
callq 0x12dd8
leaq -0x90(%rbp), %rdi
callq 0x12dd8
leaq -0xd8(%rbp), %rdi
callq 0x12dd8
movq %rbx, %rdi
callq 0x3320