// File: /workspace/github_cpp_makefile_star2_O1/DavidLBruce[P]sim3dof/build/../src/FcTseO.cpp
// Function: FcTseO::UpdateStates(double)
void FcTseO::UpdateStates( double measPosY )
{
  double   temp0;
  Matrix   temp1(3,3);
  Matrix   temp2(3,3);

//  RR = SQ( rinPosY );
// TBD RR is wrong, but it works better.  Maybe process noise or initial PP
// is too low?
  RR = rinPosY;

  //
  // Calculate the Kalman gains.
  //

//  temp0 = HH * PP * HT + RR;  
  temp0 = PP[0][0] + RR;  

  //
  //  KK = PP * HT  / temp0;
  //
  KK[0][0] = PP[0][0] / temp0;
  KK[1][0] = PP[1][0] / temp0;
  KK[2][0] = PP[2][0] / temp0;
//  KK = PP * HT  / temp0;

  //
  // Update the state with the measurement
  //
  residual = measPosY - stateVec(0);

  innovation = KK * residual;

  stateVec += innovation;

  //
  // Update the estimation covariance
  //
  //  temp1 = eye - KK * HH
  // 
  temp1 = 0.0;
  temp1[0][0] = KK[0][0];
  temp1[1][0] = KK[1][0];
  temp1[2][0] = KK[2][0];

  temp1 = eye - temp1;

  //
  // temp2 = KK * RR * KT
  //
  temp2 = 0.0;
  temp2[0][0] = KK[0][0] * KK[0][0];
  temp2[0][1] = KK[0][0] * KK[1][0];
  temp2[0][2] = KK[0][0] * KK[2][0];

  temp2[1][0] = KK[1][0] * KK[0][0];
  temp2[1][1] = KK[1][0] * KK[1][0];
  temp2[1][2] = KK[1][0] * KK[2][0];

  temp2[2][0] = KK[2][0] * KK[0][0];
  temp2[2][1] = KK[2][0] * KK[1][0];
  temp2[2][2] = KK[2][0] * KK[2][0];

  temp2 = temp2 * RR;

  PP = temp1 * PP * Transpose( temp1 ) + temp2;

}

// Assembly:
pushq %rbp
movq %rsp, %rbp
pushq %r15
pushq %r14
pushq %r13
pushq %r12
pushq %rbx
subq $0x1f8, %rsp            # imm = 0x1F8
movsd %xmm0, -0x70(%rbp)
movq %rdi, %r14
leaq -0x68(%rbp), %rdi
xorpd %xmm0, %xmm0
movl $0x3, %esi
movl $0x3, %edx
callq 0x13504
leaq -0x50(%rbp), %rdi
xorpd %xmm0, %xmm0
movl $0x3, %esi
movl $0x3, %edx
callq 0x13504
movsd 0x218(%r14), %xmm0
movsd %xmm0, 0x220(%r14)
leaq 0x1f8(%r14), %rbx
movq %rbx, %rdi
xorl %esi, %esi
callq 0x13b48
movsd (%rax), %xmm0
addsd 0x220(%r14), %xmm0
movsd %xmm0, -0x30(%rbp)
movq %rbx, %rdi
xorl %esi, %esi
callq 0x13b48
movsd (%rax), %xmm0
movsd %xmm0, -0x38(%rbp)
leaq 0x1c0(%r14), %r15
movq %r15, %rdi
xorl %esi, %esi
callq 0x13b48
movsd -0x38(%rbp), %xmm0
divsd -0x30(%rbp), %xmm0
movsd %xmm0, (%rax)
movq %rbx, %rdi
movl $0x1, %esi
callq 0x13b48
movsd (%rax), %xmm0
movsd %xmm0, -0x38(%rbp)
movq %r15, %rdi
movl $0x1, %esi
callq 0x13b48
movsd -0x38(%rbp), %xmm0
divsd -0x30(%rbp), %xmm0
movsd %xmm0, (%rax)
movq %rbx, %rdi
movl $0x2, %esi
callq 0x13b48
movsd (%rax), %xmm0
movsd %xmm0, -0x38(%rbp)
movq %r15, %rdi
movl $0x2, %esi
callq 0x13b48
movsd -0x38(%rbp), %xmm0
divsd -0x30(%rbp), %xmm0
movsd %xmm0, (%rax)
leaq 0x248(%r14), %r12
movq %r12, %rdi
xorl %esi, %esi
callq 0x13aba
movsd -0x70(%rbp), %xmm0
subsd (%rax), %xmm0
movsd %xmm0, 0x210(%r14)
leaq -0x190(%rbp), %rdi
movq %r15, %rsi
callq 0x14000
leaq 0x1a8(%r14), %r13
leaq -0x220(%rbp), %rdi
leaq -0x190(%rbp), %rdx
movq %r13, %rsi
callq 0x13b92
leaq -0x220(%rbp), %rdi
callq 0x13642
leaq -0x190(%rbp), %rdi
callq 0x13642
leaq -0x178(%rbp), %rdi
movq %r13, %rsi
callq 0x13576
leaq -0x178(%rbp), %rsi
movq %r12, %rdi
callq 0x140ee
leaq -0x178(%rbp), %rdi
callq 0x13642
leaq -0x208(%rbp), %rdi
leaq -0x68(%rbp), %rsi
xorpd %xmm0, %xmm0
callq 0x13c1a
leaq -0x208(%rbp), %rdi
callq 0x13642
movq %r15, %rdi
xorl %esi, %esi
callq 0x13b48
movsd (%rax), %xmm0
movsd %xmm0, -0x30(%rbp)
leaq -0x68(%rbp), %rdi
xorl %esi, %esi
callq 0x13b48
movsd -0x30(%rbp), %xmm0
movsd %xmm0, (%rax)
movq %r15, %rdi
movl $0x1, %esi
callq 0x13b48
movsd (%rax), %xmm0
movsd %xmm0, -0x30(%rbp)
leaq -0x68(%rbp), %rdi
movl $0x1, %esi
callq 0x13b48
movsd -0x30(%rbp), %xmm0
movsd %xmm0, (%rax)
movq %r15, %rdi
movl $0x2, %esi
callq 0x13b48
movsd (%rax), %xmm0
movsd %xmm0, -0x30(%rbp)
leaq -0x68(%rbp), %rdi
movl $0x2, %esi
callq 0x13b48
movsd -0x30(%rbp), %xmm0
movsd %xmm0, (%rax)
leaq -0x148(%rbp), %rdi
leaq -0x68(%rbp), %rsi
callq 0x13576
leaq 0x158(%r14), %rsi
leaq -0x160(%rbp), %rdi
leaq -0x148(%rbp), %rdx
callq 0x13cda
leaq -0x1f0(%rbp), %rdi
leaq -0x68(%rbp), %rsi
leaq -0x160(%rbp), %rdx
callq 0x13b92
leaq -0x1f0(%rbp), %rdi
callq 0x13642
leaq -0x160(%rbp), %rdi
callq 0x13642
leaq -0x148(%rbp), %rdi
callq 0x13642
leaq -0x1d8(%rbp), %rdi
leaq -0x50(%rbp), %rsi
xorpd %xmm0, %xmm0
callq 0x13c1a
leaq -0x1d8(%rbp), %rdi
callq 0x13642
movq %r15, %rdi
xorl %esi, %esi
callq 0x13b48
movsd (%rax), %xmm0
movsd %xmm0, -0x30(%rbp)
movq %r15, %rdi
xorl %esi, %esi
callq 0x13b48
movsd (%rax), %xmm0
movsd %xmm0, -0x38(%rbp)
leaq -0x50(%rbp), %rdi
xorl %esi, %esi
callq 0x13b48
movsd -0x30(%rbp), %xmm0
mulsd -0x38(%rbp), %xmm0
movsd %xmm0, (%rax)
movq %r15, %rdi
xorl %esi, %esi
callq 0x13b48
movsd (%rax), %xmm0
movsd %xmm0, -0x30(%rbp)
movq %r15, %rdi
movl $0x1, %esi
callq 0x13b48
movsd (%rax), %xmm0
movsd %xmm0, -0x38(%rbp)
leaq -0x50(%rbp), %rdi
xorl %esi, %esi
callq 0x13b48
movsd -0x30(%rbp), %xmm0
mulsd -0x38(%rbp), %xmm0
movsd %xmm0, 0x8(%rax)
movq %r15, %rdi
xorl %esi, %esi
callq 0x13b48
movsd (%rax), %xmm0
movsd %xmm0, -0x30(%rbp)
movq %r15, %rdi
movl $0x2, %esi
callq 0x13b48
movsd (%rax), %xmm0
movsd %xmm0, -0x38(%rbp)
leaq -0x50(%rbp), %rdi
xorl %esi, %esi
callq 0x13b48
movsd -0x30(%rbp), %xmm0
mulsd -0x38(%rbp), %xmm0
movsd %xmm0, 0x10(%rax)
movq %r15, %rdi
movl $0x1, %esi
callq 0x13b48
movsd (%rax), %xmm0
movsd %xmm0, -0x30(%rbp)
movq %r15, %rdi
xorl %esi, %esi
callq 0x13b48
movsd (%rax), %xmm0
movsd %xmm0, -0x38(%rbp)
leaq -0x50(%rbp), %rdi
movl $0x1, %esi
callq 0x13b48
movsd -0x30(%rbp), %xmm0
mulsd -0x38(%rbp), %xmm0
movsd %xmm0, (%rax)
movq %r15, %rdi
movl $0x1, %esi
callq 0x13b48
movsd (%rax), %xmm0
movsd %xmm0, -0x30(%rbp)
movq %r15, %rdi
movl $0x1, %esi
callq 0x13b48
movsd (%rax), %xmm0
movsd %xmm0, -0x38(%rbp)
leaq -0x50(%rbp), %rdi
movl $0x1, %esi
callq 0x13b48
movsd -0x30(%rbp), %xmm0
mulsd -0x38(%rbp), %xmm0
movsd %xmm0, 0x8(%rax)
movq %r15, %rdi
movl $0x1, %esi
callq 0x13b48
movsd (%rax), %xmm0
movsd %xmm0, -0x30(%rbp)
movq %r15, %rdi
movl $0x2, %esi
callq 0x13b48
movsd (%rax), %xmm0
movsd %xmm0, -0x38(%rbp)
leaq -0x50(%rbp), %rdi
movl $0x1, %esi
callq 0x13b48
movsd -0x30(%rbp), %xmm0
mulsd -0x38(%rbp), %xmm0
movsd %xmm0, 0x10(%rax)
movq %r15, %rdi
movl $0x2, %esi
callq 0x13b48
movsd (%rax), %xmm0
movsd %xmm0, -0x30(%rbp)
movq %r15, %rdi
xorl %esi, %esi
callq 0x13b48
movsd (%rax), %xmm0
movsd %xmm0, -0x38(%rbp)
leaq -0x50(%rbp), %rdi
movl $0x2, %esi
callq 0x13b48
movsd -0x30(%rbp), %xmm0
mulsd -0x38(%rbp), %xmm0
movsd %xmm0, (%rax)
movq %r15, %rdi
movl $0x2, %esi
callq 0x13b48
movsd (%rax), %xmm0
movsd %xmm0, -0x30(%rbp)
movq %r15, %rdi
movl $0x1, %esi
callq 0x13b48
movsd (%rax), %xmm0
movsd %xmm0, -0x38(%rbp)
leaq -0x50(%rbp), %rdi
movl $0x2, %esi
callq 0x13b48
movsd -0x30(%rbp), %xmm0
mulsd -0x38(%rbp), %xmm0
movsd %xmm0, 0x8(%rax)
movq %r15, %rdi
movl $0x2, %esi
callq 0x13b48
movsd (%rax), %xmm0
movsd %xmm0, -0x30(%rbp)
movq %r15, %rdi
movl $0x2, %esi
callq 0x13b48
movsd (%rax), %xmm0
movsd %xmm0, -0x38(%rbp)
leaq -0x50(%rbp), %rdi
movl $0x2, %esi
callq 0x13b48
movsd -0x30(%rbp), %xmm0
mulsd -0x38(%rbp), %xmm0
movsd %xmm0, 0x10(%rax)
movsd 0x220(%r14), %xmm0
leaq -0x130(%rbp), %rdi
leaq -0x50(%rbp), %rsi
callq 0x14000
leaq -0x1c0(%rbp), %rdi
leaq -0x50(%rbp), %rsi
leaq -0x130(%rbp), %rdx
callq 0x13b92
leaq -0x1c0(%rbp), %rdi
callq 0x13642
leaq -0x130(%rbp), %rdi
callq 0x13642
leaq -0xd0(%rbp), %rdi
movq %rbx, %rsi
callq 0x13576
leaq -0xe8(%rbp), %rdi
leaq -0x68(%rbp), %rsi
leaq -0xd0(%rbp), %rdx
callq 0x13e62
leaq -0xa0(%rbp), %rdi
leaq -0x68(%rbp), %rsi
callq 0x13576
leaq -0xb8(%rbp), %rdi
leaq -0xa0(%rbp), %rsi
callq 0x15940
leaq -0x100(%rbp), %rdi
leaq -0xe8(%rbp), %rsi
leaq -0xb8(%rbp), %rdx
callq 0x13e62
leaq -0x88(%rbp), %rdi
leaq -0x50(%rbp), %rsi
callq 0x13576
leaq -0x118(%rbp), %rdi
leaq -0x100(%rbp), %rsi
leaq -0x88(%rbp), %rdx
callq 0x13c48
leaq -0x1a8(%rbp), %rdi
leaq -0x118(%rbp), %rdx
movq %rbx, %rsi
callq 0x13b92
leaq -0x1a8(%rbp), %rdi
callq 0x13642
leaq -0x118(%rbp), %rdi
callq 0x13642
leaq -0x88(%rbp), %rdi
callq 0x13642
leaq -0x100(%rbp), %rdi
callq 0x13642
leaq -0xb8(%rbp), %rdi
callq 0x13642
leaq -0xa0(%rbp), %rdi
callq 0x13642
leaq -0xe8(%rbp), %rdi
callq 0x13642
leaq -0xd0(%rbp), %rdi
callq 0x13642
leaq -0x50(%rbp), %rdi
callq 0x13642
leaq -0x68(%rbp), %rdi
callq 0x13642
addq $0x1f8, %rsp            # imm = 0x1F8
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq
movq %rax, %rbx
leaq -0x118(%rbp), %rdi
callq 0x13642
jmp 0x5202
movq %rax, %rbx
leaq -0x88(%rbp), %rdi
callq 0x13642
jmp 0x5213
movq %rax, %rbx
leaq -0x100(%rbp), %rdi
callq 0x13642
jmp 0x5224
movq %rax, %rbx
leaq -0xb8(%rbp), %rdi
callq 0x13642
jmp 0x5235
movq %rax, %rbx
leaq -0xa0(%rbp), %rdi
callq 0x13642
leaq -0xe8(%rbp), %rdi
callq 0x13642
leaq -0xd0(%rbp), %rdi
jmp 0x52a1
movq %rax, %rbx
jmp 0x5241
movq %rax, %rbx
jmp 0x524d
jmp 0x52ad
movq %rax, %rbx
leaq -0x130(%rbp), %rdi
jmp 0x52a1
movq %rax, %rbx
leaq -0x160(%rbp), %rdi
callq 0x13642
jmp 0x5282
movq %rax, %rbx
leaq -0x148(%rbp), %rdi
jmp 0x52a1
movq %rax, %rbx
leaq -0x178(%rbp), %rdi
jmp 0x52a1
movq %rax, %rbx
leaq -0x190(%rbp), %rdi
callq 0x13642
jmp 0x52b0
movq %rax, %rbx
jmp 0x52b9
movq %rax, %rbx
leaq -0x50(%rbp), %rdi
callq 0x13642
leaq -0x68(%rbp), %rdi
callq 0x13642
movq %rbx, %rdi
callq 0x32d0