// File: /workspace/github_cpp_makefile_star2_O2/DavidLBruce[P]sim3dof/build/../src/FcTse6.cpp
// Function: FcTse6::FcTse6()
FcTse6::FcTse6()
{
  //
  // Input parameter defaults
  //
  frameTimeStep = 1.0/64.0;
  initFrameTime = 0.0;

  singerTau = 1.0e2;     // Kalman process noise tuning parameter
  singerVar = 1.0e2;     // Kalman process noise tuning parameter

  eye.Resize(6,6);
  HH.Resize(3,6);
  HT.Resize(6,3);
  innovation.Resize(6,1);
  KK.Resize(6,3);
  measVec.Resize(3,1);
  mscState.Resize(6,1);
  phi.Resize(6,6);
  PP.Resize(6,6);
  residual.Resize(3,1);
  RR.Resize(3,3);
  cartRel.Resize(6,1);
  truState.Resize(6,1);
}

// Assembly:
pushq %rbp
movq %rsp, %rbp
pushq %r15
pushq %r14
pushq %r13
pushq %r12
pushq %rbx
subq $0x58, %rsp
movq %rdi, %rbx
callq 0x9cd8
leaq 0x17064(%rip), %rax     # 0x1c9c8
movq %rax, (%rbx)
leaq 0x110(%rbx), %rdi
pushq $0x1
popq %rdx
xorps %xmm0, %xmm0
movq %rdi, -0x60(%rbp)
movl %edx, %esi
callq 0x12cf8
leaq 0x1a0(%rbx), %rdi
pushq $0x1
popq %rdx
xorps %xmm0, %xmm0
movq %rdi, -0x50(%rbp)
movl %edx, %esi
callq 0x12cf8
leaq 0x1c0(%rbx), %rdi
pushq $0x1
popq %rdx
xorps %xmm0, %xmm0
movq %rdi, -0x78(%rbp)
movl %edx, %esi
callq 0x12cf8
leaq 0x1d8(%rbx), %rdi
pushq $0x1
popq %rdx
xorps %xmm0, %xmm0
movq %rdi, -0x70(%rbp)
movl %edx, %esi
callq 0x12cf8
leaq 0x1f0(%rbx), %rdi
pushq $0x1
popq %rdx
xorps %xmm0, %xmm0
movq %rdi, -0x68(%rbp)
movl %edx, %esi
callq 0x12cf8
leaq 0x210(%rbx), %rdi
pushq $0x1
popq %rdx
xorps %xmm0, %xmm0
movq %rdi, -0x58(%rbp)
movl %edx, %esi
callq 0x12cf8
leaq 0x228(%rbx), %rdi
pushq $0x1
popq %rdx
xorps %xmm0, %xmm0
movq %rdi, -0x48(%rbp)
movl %edx, %esi
callq 0x12cf8
leaq 0x258(%rbx), %rdi
pushq $0x1
popq %rdx
xorps %xmm0, %xmm0
movq %rdi, -0x40(%rbp)
movl %edx, %esi
callq 0x12cf8
leaq 0x270(%rbx), %rdi
pushq $0x1
popq %rdx
xorps %xmm0, %xmm0
movq %rdi, -0x38(%rbp)
movl %edx, %esi
callq 0x12cf8
leaq 0x288(%rbx), %r15
pushq $0x1
popq %rdx
xorps %xmm0, %xmm0
movq %r15, %rdi
movl %edx, %esi
callq 0x12cf8
leaq 0x2a0(%rbx), %r12
pushq $0x1
popq %rdx
xorps %xmm0, %xmm0
movq %r12, %rdi
movl %edx, %esi
callq 0x12cf8
leaq 0x2c0(%rbx), %r13
pushq $0x1
popq %rdx
xorps %xmm0, %xmm0
movq %r13, %rdi
movl %edx, %esi
callq 0x12cf8
leaq 0x2e0(%rbx), %r14
pushq $0x1
popq %rdx
xorps %xmm0, %xmm0
movq %r14, %rdi
movl %edx, %esi
callq 0x12cf8
movabsq $0x3f90000000000000, %rax # imm = 0x3F90000000000000
movq %rax, 0x8(%rbx)
andq $0x0, 0xf8(%rbx)
movabsq $0x4059000000000000, %rax # imm = 0x4059000000000000
movq %rax, 0x100(%rbx)
movq %rax, 0x108(%rbx)
pushq $0x6
popq %rdx
xorps %xmm0, %xmm0
movq -0x78(%rbp), %rdi
movl %edx, %esi
callq 0x12dfc
pushq $0x3
popq %rsi
pushq $0x6
popq %rdx
xorps %xmm0, %xmm0
movq -0x70(%rbp), %rdi
callq 0x12dfc
pushq $0x6
popq %rsi
pushq $0x3
popq %rdx
xorps %xmm0, %xmm0
movq -0x68(%rbp), %rdi
callq 0x12dfc
pushq $0x6
popq %rsi
pushq $0x1
popq %rdx
xorps %xmm0, %xmm0
movq -0x58(%rbp), %rdi
callq 0x12dfc
pushq $0x6
popq %rsi
pushq $0x3
popq %rdx
xorps %xmm0, %xmm0
movq -0x48(%rbp), %rdi
callq 0x12dfc
pushq $0x3
popq %rsi
pushq $0x1
popq %rdx
xorps %xmm0, %xmm0
movq -0x40(%rbp), %rdi
callq 0x12dfc
pushq $0x6
popq %rsi
pushq $0x1
popq %rdx
xorps %xmm0, %xmm0
movq -0x38(%rbp), %rdi
callq 0x12dfc
pushq $0x6
popq %rdx
xorps %xmm0, %xmm0
movq %r15, %rdi
movl %edx, %esi
callq 0x12dfc
pushq $0x6
popq %rdx
xorps %xmm0, %xmm0
movq %r12, %rdi
movl %edx, %esi
callq 0x12dfc
pushq $0x3
popq %rsi
pushq $0x1
popq %rdx
xorps %xmm0, %xmm0
movq %r13, %rdi
callq 0x12dfc
pushq $0x3
popq %rdx
xorps %xmm0, %xmm0
movq %r14, %rdi
movl %edx, %esi
callq 0x12dfc
pushq $0x6
popq %rsi
pushq $0x1
popq %rdx
xorps %xmm0, %xmm0
movq -0x60(%rbp), %rdi
callq 0x12dfc
pushq $0x6
popq %rsi
pushq $0x1
popq %rdx
xorps %xmm0, %xmm0
movq -0x50(%rbp), %rdi
callq 0x12dfc
addq $0x58, %rsp
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq
movq %rax, -0x30(%rbp)
jmp 0x5c14
movq %rax, -0x30(%rbp)
jmp 0x5c1c
movq %rax, -0x30(%rbp)
jmp 0x5c24
movq %rax, -0x30(%rbp)
jmp 0x5c2c
movq %rax, -0x30(%rbp)
jmp 0x5c35
movq %rax, -0x30(%rbp)
jmp 0x5c3e
movq %rax, -0x30(%rbp)
jmp 0x5c47
movq %rax, -0x30(%rbp)
jmp 0x5c50
movq %rax, -0x30(%rbp)
jmp 0x5c59
movq %rax, -0x30(%rbp)
jmp 0x5c62
movq %rax, -0x30(%rbp)
jmp 0x5c6b
movq %rax, -0x30(%rbp)
jmp 0x5c74
movq %rax, -0x30(%rbp)
jmp 0x5c7d
movq %rax, -0x30(%rbp)
movq %r14, %rdi
callq 0x12dd8
movq %r13, %rdi
callq 0x12dd8
movq %r12, %rdi
callq 0x12dd8
movq %r15, %rdi
callq 0x12dd8
movq -0x38(%rbp), %rdi
callq 0x12dd8
movq -0x40(%rbp), %rdi
callq 0x12dd8
movq -0x48(%rbp), %rdi
callq 0x12dd8
movq -0x58(%rbp), %rdi
callq 0x12dd8
movq -0x68(%rbp), %rdi
callq 0x12dd8
movq -0x70(%rbp), %rdi
callq 0x12dd8
movq -0x78(%rbp), %rdi
callq 0x12dd8
movq -0x50(%rbp), %rdi
callq 0x12dd8
movq -0x60(%rbp), %rdi
callq 0x12dd8
movq %rbx, %rdi
callq 0x9dd6
movq -0x30(%rbp), %rdi
callq 0x3320