// File: /workspace/github_cpp_makefile_star2_O2/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
pushq $0x3
popq %rbx
xorpd %xmm0, %xmm0
movl %ebx, %esi
movl %ebx, %edx
callq 0x12cf8
leaq -0x50(%rbp), %rdi
xorpd %xmm0, %xmm0
movl %ebx, %esi
movl %ebx, %edx
callq 0x12cf8
movsd 0x218(%r14), %xmm0
movsd %xmm0, 0x220(%r14)
leaq 0x1f8(%r14), %rbx
movq %rbx, %rdi
xorl %esi, %esi
callq 0x1324e
movsd (%rax), %xmm0
addsd 0x220(%r14), %xmm0
movsd %xmm0, -0x30(%rbp)
movq %rbx, %rdi
xorl %esi, %esi
callq 0x1324e
movsd (%rax), %xmm0
movsd %xmm0, -0x38(%rbp)
leaq 0x1c0(%r14), %r15
movq %r15, %rdi
xorl %esi, %esi
callq 0x1324e
movsd -0x38(%rbp), %xmm0
divsd -0x30(%rbp), %xmm0
movsd %xmm0, (%rax)
pushq $0x1
popq %rsi
movq %rbx, %rdi
callq 0x1324e
movsd (%rax), %xmm0
movsd %xmm0, -0x38(%rbp)
pushq $0x1
popq %rsi
movq %r15, %rdi
callq 0x1324e
movsd -0x38(%rbp), %xmm0
divsd -0x30(%rbp), %xmm0
movsd %xmm0, (%rax)
pushq $0x2
popq %rsi
movq %rbx, %rdi
callq 0x1324e
movsd (%rax), %xmm0
movsd %xmm0, -0x38(%rbp)
pushq $0x2
popq %rsi
movq %r15, %rdi
callq 0x1324e
movsd -0x38(%rbp), %xmm0
divsd -0x30(%rbp), %xmm0
movsd %xmm0, (%rax)
leaq 0x248(%r14), %r12
movq %r12, %rdi
xorl %esi, %esi
callq 0x131c0
movsd -0x70(%rbp), %xmm0
subsd (%rax), %xmm0
movsd %xmm0, 0x210(%r14)
leaq -0x190(%rbp), %rdi
movq %r15, %rsi
callq 0x136c2
leaq 0x1a8(%r14), %r13
leaq -0x220(%rbp), %rdi
leaq -0x190(%rbp), %rdx
movq %r13, %rsi
callq 0x13296
leaq -0x220(%rbp), %rdi
callq 0x12dd8
leaq -0x190(%rbp), %rdi
callq 0x12dd8
leaq -0x178(%rbp), %rdi
movq %r13, %rsi
callq 0x12d48
leaq -0x178(%rbp), %rsi
movq %r12, %rdi
callq 0x137b0
leaq -0x178(%rbp), %rdi
callq 0x12dd8
leaq -0x208(%rbp), %rdi
leaq -0x68(%rbp), %rsi
xorpd %xmm0, %xmm0
callq 0x132f0
leaq -0x208(%rbp), %rdi
callq 0x12dd8
movq %r15, %rdi
xorl %esi, %esi
callq 0x1324e
movsd (%rax), %xmm0
movsd %xmm0, -0x30(%rbp)
leaq -0x68(%rbp), %rdi
xorl %esi, %esi
callq 0x1324e
movsd -0x30(%rbp), %xmm0
movsd %xmm0, (%rax)
pushq $0x1
popq %rsi
movq %r15, %rdi
callq 0x1324e
movsd (%rax), %xmm0
movsd %xmm0, -0x30(%rbp)
leaq -0x68(%rbp), %rdi
pushq $0x1
popq %rsi
callq 0x1324e
movsd -0x30(%rbp), %xmm0
movsd %xmm0, (%rax)
pushq $0x2
popq %rsi
movq %r15, %rdi
callq 0x1324e
movsd (%rax), %xmm0
movsd %xmm0, -0x30(%rbp)
leaq -0x68(%rbp), %rdi
pushq $0x2
popq %rsi
callq 0x1324e
movsd -0x30(%rbp), %xmm0
movsd %xmm0, (%rax)
leaq -0x148(%rbp), %rdi
leaq -0x68(%rbp), %rsi
callq 0x12d48
leaq 0x158(%r14), %rsi
leaq -0x160(%rbp), %rdi
leaq -0x148(%rbp), %rdx
callq 0x133aa
leaq -0x1f0(%rbp), %rdi
leaq -0x68(%rbp), %rsi
leaq -0x160(%rbp), %rdx
callq 0x13296
leaq -0x1f0(%rbp), %rdi
callq 0x12dd8
leaq -0x160(%rbp), %rdi
callq 0x12dd8
leaq -0x148(%rbp), %rdi
callq 0x12dd8
leaq -0x1d8(%rbp), %rdi
leaq -0x50(%rbp), %rsi
xorpd %xmm0, %xmm0
callq 0x132f0
leaq -0x1d8(%rbp), %rdi
callq 0x12dd8
movq %r15, %rdi
xorl %esi, %esi
callq 0x1324e
movsd (%rax), %xmm0
movsd %xmm0, -0x30(%rbp)
movq %r15, %rdi
xorl %esi, %esi
callq 0x1324e
movsd (%rax), %xmm0
movsd %xmm0, -0x38(%rbp)
leaq -0x50(%rbp), %rdi
xorl %esi, %esi
callq 0x1324e
movsd -0x30(%rbp), %xmm0
mulsd -0x38(%rbp), %xmm0
movsd %xmm0, (%rax)
movq %r15, %rdi
xorl %esi, %esi
callq 0x1324e
movsd (%rax), %xmm0
movsd %xmm0, -0x30(%rbp)
pushq $0x1
popq %rsi
movq %r15, %rdi
callq 0x1324e
movsd (%rax), %xmm0
movsd %xmm0, -0x38(%rbp)
leaq -0x50(%rbp), %rdi
xorl %esi, %esi
callq 0x1324e
movsd -0x30(%rbp), %xmm0
mulsd -0x38(%rbp), %xmm0
movsd %xmm0, 0x8(%rax)
movq %r15, %rdi
xorl %esi, %esi
callq 0x1324e
movsd (%rax), %xmm0
movsd %xmm0, -0x30(%rbp)
pushq $0x2
popq %rsi
movq %r15, %rdi
callq 0x1324e
movsd (%rax), %xmm0
movsd %xmm0, -0x38(%rbp)
leaq -0x50(%rbp), %rdi
xorl %esi, %esi
callq 0x1324e
movsd -0x30(%rbp), %xmm0
mulsd -0x38(%rbp), %xmm0
movsd %xmm0, 0x10(%rax)
pushq $0x1
popq %rsi
movq %r15, %rdi
callq 0x1324e
movsd (%rax), %xmm0
movsd %xmm0, -0x30(%rbp)
movq %r15, %rdi
xorl %esi, %esi
callq 0x1324e
movsd (%rax), %xmm0
movsd %xmm0, -0x38(%rbp)
leaq -0x50(%rbp), %rdi
pushq $0x1
popq %rsi
callq 0x1324e
movsd -0x30(%rbp), %xmm0
mulsd -0x38(%rbp), %xmm0
movsd %xmm0, (%rax)
pushq $0x1
popq %rsi
movq %r15, %rdi
callq 0x1324e
movsd (%rax), %xmm0
movsd %xmm0, -0x30(%rbp)
pushq $0x1
popq %rsi
movq %r15, %rdi
callq 0x1324e
movsd (%rax), %xmm0
movsd %xmm0, -0x38(%rbp)
leaq -0x50(%rbp), %rdi
pushq $0x1
popq %rsi
callq 0x1324e
movsd -0x30(%rbp), %xmm0
mulsd -0x38(%rbp), %xmm0
movsd %xmm0, 0x8(%rax)
pushq $0x1
popq %rsi
movq %r15, %rdi
callq 0x1324e
movsd (%rax), %xmm0
movsd %xmm0, -0x30(%rbp)
pushq $0x2
popq %rsi
movq %r15, %rdi
callq 0x1324e
movsd (%rax), %xmm0
movsd %xmm0, -0x38(%rbp)
leaq -0x50(%rbp), %rdi
pushq $0x1
popq %rsi
callq 0x1324e
movsd -0x30(%rbp), %xmm0
mulsd -0x38(%rbp), %xmm0
movsd %xmm0, 0x10(%rax)
pushq $0x2
popq %rsi
movq %r15, %rdi
callq 0x1324e
movsd (%rax), %xmm0
movsd %xmm0, -0x30(%rbp)
movq %r15, %rdi
xorl %esi, %esi
callq 0x1324e
movsd (%rax), %xmm0
movsd %xmm0, -0x38(%rbp)
leaq -0x50(%rbp), %rdi
pushq $0x2
popq %rsi
callq 0x1324e
movsd -0x30(%rbp), %xmm0
mulsd -0x38(%rbp), %xmm0
movsd %xmm0, (%rax)
pushq $0x2
popq %rsi
movq %r15, %rdi
callq 0x1324e
movsd (%rax), %xmm0
movsd %xmm0, -0x30(%rbp)
pushq $0x1
popq %rsi
movq %r15, %rdi
callq 0x1324e
movsd (%rax), %xmm0
movsd %xmm0, -0x38(%rbp)
leaq -0x50(%rbp), %rdi
pushq $0x2
popq %rsi
callq 0x1324e
movsd -0x30(%rbp), %xmm0
mulsd -0x38(%rbp), %xmm0
movsd %xmm0, 0x8(%rax)
pushq $0x2
popq %rsi
movq %r15, %rdi
callq 0x1324e
movsd (%rax), %xmm0
movsd %xmm0, -0x30(%rbp)
pushq $0x2
popq %rsi
movq %r15, %rdi
callq 0x1324e
movsd (%rax), %xmm0
movsd %xmm0, -0x38(%rbp)
leaq -0x50(%rbp), %rdi
pushq $0x2
popq %rsi
callq 0x1324e
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 0x136c2
leaq -0x1c0(%rbp), %rdi
leaq -0x50(%rbp), %rsi
leaq -0x130(%rbp), %rdx
callq 0x13296
leaq -0x1c0(%rbp), %rdi
callq 0x12dd8
leaq -0x130(%rbp), %rdi
callq 0x12dd8
leaq -0xd0(%rbp), %rdi
movq %rbx, %rsi
callq 0x12d48
leaq -0xe8(%rbp), %rdi
leaq -0x68(%rbp), %rsi
leaq -0xd0(%rbp), %rdx
callq 0x13512
leaq -0xa0(%rbp), %rdi
leaq -0x68(%rbp), %rsi
callq 0x12d48
leaq -0xb8(%rbp), %rdi
leaq -0xa0(%rbp), %rsi
callq 0x14d68
leaq -0x100(%rbp), %rdi
leaq -0xe8(%rbp), %rsi
leaq -0xb8(%rbp), %rdx
callq 0x13512
leaq -0x88(%rbp), %rdi
leaq -0x50(%rbp), %rsi
callq 0x12d48
leaq -0x118(%rbp), %rdi
leaq -0x100(%rbp), %rsi
leaq -0x88(%rbp), %rdx
callq 0x1331e
leaq -0x1a8(%rbp), %rdi
leaq -0x118(%rbp), %rdx
movq %rbx, %rsi
callq 0x13296
leaq -0x1a8(%rbp), %rdi
callq 0x12dd8
leaq -0x118(%rbp), %rdi
callq 0x12dd8
leaq -0x88(%rbp), %rdi
callq 0x12dd8
leaq -0x100(%rbp), %rdi
callq 0x12dd8
leaq -0xb8(%rbp), %rdi
callq 0x12dd8
leaq -0xa0(%rbp), %rdi
callq 0x12dd8
leaq -0xe8(%rbp), %rdi
callq 0x12dd8
leaq -0xd0(%rbp), %rdi
callq 0x12dd8
leaq -0x50(%rbp), %rdi
callq 0x12dd8
leaq -0x68(%rbp), %rdi
callq 0x12dd8
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 0x12dd8
jmp 0x515d
movq %rax, %rbx
leaq -0x88(%rbp), %rdi
callq 0x12dd8
jmp 0x516e
movq %rax, %rbx
leaq -0x100(%rbp), %rdi
callq 0x12dd8
jmp 0x517f
movq %rax, %rbx
leaq -0xb8(%rbp), %rdi
callq 0x12dd8
jmp 0x5190
movq %rax, %rbx
leaq -0xa0(%rbp), %rdi
callq 0x12dd8
leaq -0xe8(%rbp), %rdi
callq 0x12dd8
leaq -0xd0(%rbp), %rdi
jmp 0x51fc
movq %rax, %rbx
jmp 0x519c
movq %rax, %rbx
jmp 0x51a8
jmp 0x5208
movq %rax, %rbx
leaq -0x130(%rbp), %rdi
jmp 0x51fc
movq %rax, %rbx
leaq -0x160(%rbp), %rdi
callq 0x12dd8
jmp 0x51dd
movq %rax, %rbx
leaq -0x148(%rbp), %rdi
jmp 0x51fc
movq %rax, %rbx
leaq -0x178(%rbp), %rdi
jmp 0x51fc
movq %rax, %rbx
leaq -0x190(%rbp), %rdi
callq 0x12dd8
jmp 0x520b
movq %rax, %rbx
jmp 0x5214
movq %rax, %rbx
leaq -0x50(%rbp), %rdi
callq 0x12dd8
leaq -0x68(%rbp), %rdi
callq 0x12dd8
movq %rbx, %rdi
callq 0x3320
nop