// File: /workspace/github_cpp_makefile_star2_O1/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
xorpd %xmm0, %xmm0
movl $0x3, %esi
movl $0x3, %edx
callq 0x13504
leaq -0x90(%rbp), %rdi
xorpd %xmm0, %xmm0
movl $0x3, %esi
movl $0x3, %edx
callq 0x13504
leaq -0xc0(%rbp), %rdi
xorpd %xmm0, %xmm0
movl $0x6, %esi
movl $0x6, %edx
callq 0x13504
movsd 0x2d8(%r14), %xmm0
movsd %xmm0, -0x30(%rbp)
leaq 0x258(%r14), %r12
movq %r12, %rdi
movl $0x1, %esi
callq 0x13aba
movsd (%rax), %xmm0
movsd %xmm0, -0x38(%rbp)
movsd 0x2b8(%r14), %xmm0
movsd %xmm0, -0x40(%rbp)
movsd 0x2d8(%r14), %xmm0
movsd %xmm0, -0x48(%rbp)
movq %r12, %rdi
movl $0x2, %esi
callq 0x13aba
movsd (%rax), %xmm0
movsd %xmm0, -0xa0(%rbp)
movq %r12, %rdi
movl $0x1, %esi
callq 0x13aba
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 0x13b48
movsd -0xa8(%rbp), %xmm0
mulsd %xmm0, %xmm0
movsd %xmm0, (%rax)
movq %rbx, %rdi
movl $0x1, %esi
callq 0x13b48
movsd -0x38(%rbp), %xmm1
mulsd %xmm1, %xmm1
movsd -0x30(%rbp), %xmm0
mulsd %xmm1, %xmm0
mulsd %xmm0, %xmm0
movsd %xmm0, 0x8(%rax)
movq %rbx, %rdi
movl $0x2, %esi
callq 0x13b48
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)
movq %r12, %rdi
movl $0x2, %esi
callq 0x13aba
movsd (%rax), %xmm0
movsd %xmm0, -0x38(%rbp)
movq %r12, %rdi
movl $0x1, %esi
callq 0x13aba
movsd (%rax), %xmm0
movsd %xmm0, -0x48(%rbp)
movq %r12, %rdi
movl $0x1, %esi
callq 0x13aba
movsd (%rax), %xmm0
movsd %xmm0, -0x40(%rbp)
movq %rbx, %rdi
movl $0x1, %esi
callq 0x13b48
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)
movq %rbx, %rdi
movl $0x1, %esi
callq 0x13b48
movsd 0x10(%rax), %xmm0
movsd %xmm0, -0x30(%rbp)
movq %rbx, %rdi
movl $0x2, %esi
callq 0x13b48
movsd -0x30(%rbp), %xmm0
movsd %xmm0, 0x8(%rax)
leaq 0x2a0(%r14), %rsi
leaq -0x330(%rbp), %rdi
movq %rsi, -0x30(%rbp)
callq 0x13576
leaq 0x1d8(%r14), %rsi
leaq -0x78(%rbp), %rdi
leaq -0x330(%rbp), %rdx
movq %rsi, -0x38(%rbp)
callq 0x13e62
leaq 0x1f0(%r14), %r15
leaq -0x318(%rbp), %rdi
movq %r15, %rsi
callq 0x13576
leaq -0x60(%rbp), %rdi
leaq -0x78(%rbp), %rsi
leaq -0x318(%rbp), %rdx
callq 0x13e62
leaq -0x300(%rbp), %rdi
movq %rbx, %rsi
callq 0x13576
leaq -0x348(%rbp), %rdi
leaq -0x60(%rbp), %rsi
leaq -0x300(%rbp), %rdx
callq 0x13c48
leaq -0x3f0(%rbp), %rdi
leaq -0x90(%rbp), %rsi
leaq -0x348(%rbp), %rdx
callq 0x13b92
leaq -0x3f0(%rbp), %rdi
callq 0x13642
leaq -0x348(%rbp), %rdi
callq 0x13642
leaq -0x300(%rbp), %rdi
callq 0x13642
leaq -0x60(%rbp), %rdi
callq 0x13642
leaq -0x318(%rbp), %rdi
callq 0x13642
leaq -0x78(%rbp), %rdi
callq 0x13642
leaq -0x330(%rbp), %rdi
callq 0x13642
leaq -0x90(%rbp), %rdi
leaq -0xd8(%rbp), %rsi
callq 0x881f
leaq -0x2e8(%rbp), %rdi
leaq -0xd8(%rbp), %rsi
callq 0x13576
leaq -0x3d8(%rbp), %rdi
leaq -0x90(%rbp), %rsi
leaq -0x2e8(%rbp), %rdx
callq 0x13b92
leaq -0x3d8(%rbp), %rdi
callq 0x13642
leaq -0x2e8(%rbp), %rdi
callq 0x13642
leaq -0x2b8(%rbp), %rdi
movq %r15, %rsi
callq 0x13576
leaq -0x60(%rbp), %rdi
leaq -0x2b8(%rbp), %rdx
movq -0x30(%rbp), %rsi
callq 0x13e62
leaq -0x2a0(%rbp), %rdi
leaq -0x90(%rbp), %rsi
callq 0x13576
leaq -0x2d0(%rbp), %rdi
leaq -0x60(%rbp), %rsi
leaq -0x2a0(%rbp), %rdx
callq 0x13e62
leaq 0x228(%r14), %r15
leaq -0x3c0(%rbp), %rdi
leaq -0x2d0(%rbp), %rdx
movq %r15, %rsi
callq 0x13b92
leaq -0x3c0(%rbp), %rdi
callq 0x13642
leaq -0x2d0(%rbp), %rdi
callq 0x13642
leaq -0x2a0(%rbp), %rdi
callq 0x13642
leaq -0x60(%rbp), %rdi
callq 0x13642
leaq -0x2b8(%rbp), %rdi
callq 0x13642
leaq 0x270(%r14), %r13
leaq -0x258(%rbp), %rdi
movq %r13, %rsi
callq 0x13576
leaq -0x270(%rbp), %rdi
leaq -0x258(%rbp), %rdx
movq -0x38(%rbp), %rsi
callq 0x13e62
leaq -0x288(%rbp), %rdi
leaq -0x270(%rbp), %rdx
movq %r12, %rsi
callq 0x13cda
leaq 0x2c0(%r14), %r12
leaq -0x3a8(%rbp), %rdi
leaq -0x288(%rbp), %rdx
movq %r12, %rsi
callq 0x13b92
leaq -0x3a8(%rbp), %rdi
callq 0x13642
leaq -0x288(%rbp), %rdi
callq 0x13642
leaq -0x270(%rbp), %rdi
callq 0x13642
leaq -0x258(%rbp), %rdi
callq 0x13642
leaq -0x228(%rbp), %rdi
movq %r12, %rsi
callq 0x13576
leaq -0x240(%rbp), %rdi
leaq -0x228(%rbp), %rdx
movq %r15, %rsi
callq 0x13e62
leaq 0x210(%r14), %r12
leaq -0x390(%rbp), %rdi
leaq -0x240(%rbp), %rdx
movq %r12, %rsi
callq 0x13b92
leaq -0x390(%rbp), %rdi
callq 0x13642
leaq -0x240(%rbp), %rdi
callq 0x13642
leaq -0x228(%rbp), %rdi
callq 0x13642
leaq -0x210(%rbp), %rdi
movq %r12, %rsi
callq 0x13576
leaq -0x210(%rbp), %rsi
movq %r13, %rdi
callq 0x140ee
leaq -0x210(%rbp), %rdi
callq 0x13642
leaq -0x1c8(%rbp), %rdi
movq -0x38(%rbp), %rsi
callq 0x13576
leaq -0x1e0(%rbp), %rdi
leaq -0x1c8(%rbp), %rdx
movq %r15, %rsi
callq 0x13e62
addq $0x1c0, %r14            # imm = 0x1C0
leaq -0x1f8(%rbp), %rdi
leaq -0x1e0(%rbp), %rdx
movq %r14, %rsi
callq 0x13cda
leaq -0x378(%rbp), %rdi
leaq -0xc0(%rbp), %rsi
leaq -0x1f8(%rbp), %rdx
callq 0x13b92
leaq -0x378(%rbp), %rdi
callq 0x13642
leaq -0x1f8(%rbp), %rdi
callq 0x13642
leaq -0x1e0(%rbp), %rdi
callq 0x13642
leaq -0x1c8(%rbp), %rdi
callq 0x13642
leaq -0x198(%rbp), %rdi
movq -0x30(%rbp), %rsi
callq 0x13576
leaq -0x78(%rbp), %rdi
leaq -0xc0(%rbp), %rsi
leaq -0x198(%rbp), %rdx
callq 0x13e62
leaq -0x168(%rbp), %rdi
leaq -0xc0(%rbp), %rsi
callq 0x13576
leaq -0x180(%rbp), %rdi
leaq -0x168(%rbp), %rsi
callq 0x15940
leaq -0x60(%rbp), %rdi
leaq -0x78(%rbp), %rsi
leaq -0x180(%rbp), %rdx
callq 0x13e62
leaq -0x120(%rbp), %rdi
movq %rbx, %rsi
callq 0x13576
leaq -0x138(%rbp), %rdi
leaq -0x120(%rbp), %rdx
movq %r15, %rsi
callq 0x13e62
leaq -0xf0(%rbp), %rdi
movq %r15, %rsi
callq 0x13576
leaq -0x108(%rbp), %rdi
leaq -0xf0(%rbp), %rsi
callq 0x15940
leaq -0x150(%rbp), %rdi
leaq -0x138(%rbp), %rsi
leaq -0x108(%rbp), %rdx
callq 0x13e62
leaq -0x1b0(%rbp), %rdi
leaq -0x60(%rbp), %rsi
leaq -0x150(%rbp), %rdx
callq 0x13c48
leaq -0x360(%rbp), %rdi
leaq -0x1b0(%rbp), %rdx
movq -0x30(%rbp), %rsi
callq 0x13b92
leaq -0x360(%rbp), %rdi
callq 0x13642
leaq -0x1b0(%rbp), %rdi
callq 0x13642
leaq -0x150(%rbp), %rdi
callq 0x13642
leaq -0x108(%rbp), %rdi
callq 0x13642
leaq -0xf0(%rbp), %rdi
callq 0x13642
leaq -0x138(%rbp), %rdi
callq 0x13642
leaq -0x120(%rbp), %rdi
callq 0x13642
leaq -0x60(%rbp), %rdi
callq 0x13642
leaq -0x180(%rbp), %rdi
callq 0x13642
leaq -0x168(%rbp), %rdi
callq 0x13642
leaq -0x78(%rbp), %rdi
callq 0x13642
leaq -0x198(%rbp), %rdi
callq 0x13642
leaq -0xc0(%rbp), %rdi
callq 0x13642
leaq -0x90(%rbp), %rdi
callq 0x13642
leaq -0xd8(%rbp), %rdi
callq 0x13642
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 0x13642
jmp 0x75e2
movq %rax, %rbx
leaq -0x150(%rbp), %rdi
callq 0x13642
jmp 0x75f3
movq %rax, %rbx
leaq -0x108(%rbp), %rdi
callq 0x13642
jmp 0x7604
movq %rax, %rbx
leaq -0xf0(%rbp), %rdi
callq 0x13642
jmp 0x7615
movq %rax, %rbx
leaq -0x138(%rbp), %rdi
callq 0x13642
leaq -0x120(%rbp), %rdi
callq 0x13642
leaq -0x60(%rbp), %rdi
callq 0x13642
leaq -0x180(%rbp), %rdi
callq 0x13642
leaq -0x168(%rbp), %rdi
callq 0x13642
leaq -0x78(%rbp), %rdi
callq 0x13642
leaq -0x198(%rbp), %rdi
jmp 0x77b5
movq %rax, %rbx
jmp 0x7621
movq %rax, %rbx
jmp 0x762d
movq %rax, %rbx
jmp 0x7636
movq %rax, %rbx
jmp 0x7642
movq %rax, %rbx
jmp 0x764e
movq %rax, %rbx
jmp 0x7657
jmp 0x77cd
movq %rax, %rbx
leaq -0x1f8(%rbp), %rdi
callq 0x13642
jmp 0x769a
movq %rax, %rbx
leaq -0x1e0(%rbp), %rdi
callq 0x13642
jmp 0x76ab
movq %rax, %rbx
leaq -0x1c8(%rbp), %rdi
jmp 0x77b5
movq %rax, %rbx
leaq -0x210(%rbp), %rdi
jmp 0x77b5
movq %rax, %rbx
leaq -0x240(%rbp), %rdi
callq 0x13642
jmp 0x76da
movq %rax, %rbx
leaq -0x228(%rbp), %rdi
jmp 0x77b5
movq %rax, %rbx
leaq -0x288(%rbp), %rdi
callq 0x13642
jmp 0x76fa
movq %rax, %rbx
leaq -0x270(%rbp), %rdi
callq 0x13642
jmp 0x770b
movq %rax, %rbx
leaq -0x258(%rbp), %rdi
jmp 0x77b5
movq %rax, %rbx
leaq -0x2d0(%rbp), %rdi
callq 0x13642
jmp 0x772b
movq %rax, %rbx
leaq -0x2a0(%rbp), %rdi
callq 0x13642
jmp 0x773c
movq %rax, %rbx
leaq -0x60(%rbp), %rdi
callq 0x13642
jmp 0x774a
movq %rax, %rbx
leaq -0x2b8(%rbp), %rdi
jmp 0x77b5
jmp 0x77cd
movq %rax, %rbx
leaq -0x2e8(%rbp), %rdi
jmp 0x77b5
movq %rax, %rbx
leaq -0x348(%rbp), %rdi
callq 0x13642
jmp 0x7775
movq %rax, %rbx
leaq -0x300(%rbp), %rdi
callq 0x13642
jmp 0x7786
movq %rax, %rbx
leaq -0x60(%rbp), %rdi
callq 0x13642
jmp 0x7794
movq %rax, %rbx
leaq -0x318(%rbp), %rdi
callq 0x13642
jmp 0x77a5
movq %rax, %rbx
leaq -0x78(%rbp), %rdi
callq 0x13642
leaq -0x330(%rbp), %rdi
callq 0x13642
jmp 0x77d0
movq %rax, %rbx
jmp 0x77ae
jmp 0x77cd
movq %rax, %rbx
jmp 0x77dc
movq %rax, %rbx
jmp 0x77e8
movq %rax, %rbx
leaq -0xc0(%rbp), %rdi
callq 0x13642
leaq -0x90(%rbp), %rdi
callq 0x13642
leaq -0xd8(%rbp), %rdi
callq 0x13642
movq %rbx, %rdi
callq 0x32d0