// File: 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
subq $0x90, %rsp
movq %rdi, -0x8(%rbp)
movq -0x8(%rbp), %rdi
movq %rdi, -0x28(%rbp)
callq 0xd640
movq -0x28(%rbp), %rdi
leaq 0x27ce1(%rip), %rax     # 0x2e978
movq %rax, (%rdi)
addq $0x110, %rdi            # imm = 0x110
movq %rdi, -0x20(%rbp)
movl $0x1, %edx
xorps %xmm0, %xmm0
movl %edx, %esi
callq 0x1f2e0
jmp 0x6cb6
movq -0x28(%rbp), %rdi
addq $0x1a0, %rdi            # imm = 0x1A0
movq %rdi, -0x30(%rbp)
movl $0x1, %edx
xorps %xmm0, %xmm0
movl %edx, %esi
callq 0x1f2e0
jmp 0x6cd6
movq -0x28(%rbp), %rdi
addq $0x1c0, %rdi            # imm = 0x1C0
movq %rdi, -0x38(%rbp)
movl $0x1, %edx
xorps %xmm0, %xmm0
movl %edx, %esi
callq 0x1f2e0
jmp 0x6cf6
movq -0x28(%rbp), %rdi
addq $0x1d8, %rdi            # imm = 0x1D8
movq %rdi, -0x40(%rbp)
movl $0x1, %edx
xorps %xmm0, %xmm0
movl %edx, %esi
callq 0x1f2e0
jmp 0x6d16
movq -0x28(%rbp), %rdi
addq $0x1f0, %rdi            # imm = 0x1F0
movq %rdi, -0x48(%rbp)
movl $0x1, %edx
xorps %xmm0, %xmm0
movl %edx, %esi
callq 0x1f2e0
jmp 0x6d36
movq -0x28(%rbp), %rdi
addq $0x210, %rdi            # imm = 0x210
movq %rdi, -0x50(%rbp)
movl $0x1, %edx
xorps %xmm0, %xmm0
movl %edx, %esi
callq 0x1f2e0
jmp 0x6d56
movq -0x28(%rbp), %rdi
addq $0x228, %rdi            # imm = 0x228
movq %rdi, -0x58(%rbp)
movl $0x1, %edx
xorps %xmm0, %xmm0
movl %edx, %esi
callq 0x1f2e0
jmp 0x6d76
movq -0x28(%rbp), %rdi
addq $0x258, %rdi            # imm = 0x258
movq %rdi, -0x60(%rbp)
movl $0x1, %edx
xorps %xmm0, %xmm0
movl %edx, %esi
callq 0x1f2e0
jmp 0x6d96
movq -0x28(%rbp), %rdi
addq $0x270, %rdi            # imm = 0x270
movq %rdi, -0x68(%rbp)
movl $0x1, %edx
xorps %xmm0, %xmm0
movl %edx, %esi
callq 0x1f2e0
jmp 0x6db6
movq -0x28(%rbp), %rdi
addq $0x288, %rdi            # imm = 0x288
movq %rdi, -0x70(%rbp)
movl $0x1, %edx
xorps %xmm0, %xmm0
movl %edx, %esi
callq 0x1f2e0
jmp 0x6dd6
movq -0x28(%rbp), %rdi
addq $0x2a0, %rdi            # imm = 0x2A0
movq %rdi, -0x78(%rbp)
movl $0x1, %edx
xorps %xmm0, %xmm0
movl %edx, %esi
callq 0x1f2e0
jmp 0x6df6
movq -0x28(%rbp), %rdi
addq $0x2c0, %rdi            # imm = 0x2C0
movq %rdi, -0x80(%rbp)
movl $0x1, %edx
xorps %xmm0, %xmm0
movl %edx, %esi
callq 0x1f2e0
jmp 0x6e16
movq -0x28(%rbp), %rdi
addq $0x2e0, %rdi            # imm = 0x2E0
movq %rdi, -0x88(%rbp)
movl $0x1, %edx
xorps %xmm0, %xmm0
movl %edx, %esi
callq 0x1f2e0
jmp 0x6e39
movq -0x28(%rbp), %rdi
movabsq $0x3f90000000000000, %rax # imm = 0x3F90000000000000
movq %rax, 0x8(%rdi)
movq $0x0, 0xf8(%rdi)
movabsq $0x4059000000000000, %rax # imm = 0x4059000000000000
movq %rax, 0x100(%rdi)
movq %rax, 0x108(%rdi)
addq $0x1c0, %rdi            # imm = 0x1C0
movl $0x6, %edx
xorps %xmm0, %xmm0
movl %edx, %esi
callq 0x1f5c0
jmp 0x6e86
movq -0x28(%rbp), %rdi
addq $0x1d8, %rdi            # imm = 0x1D8
movl $0x3, %esi
movl $0x6, %edx
xorps %xmm0, %xmm0
callq 0x1f5c0
jmp 0x6ea5
movq -0x28(%rbp), %rdi
addq $0x1f0, %rdi            # imm = 0x1F0
movl $0x6, %esi
movl $0x3, %edx
xorps %xmm0, %xmm0
callq 0x1f5c0
jmp 0x6ec4
movq -0x28(%rbp), %rdi
addq $0x210, %rdi            # imm = 0x210
movl $0x6, %esi
movl $0x1, %edx
xorps %xmm0, %xmm0
callq 0x1f5c0
jmp 0x6ee3
movq -0x28(%rbp), %rdi
addq $0x228, %rdi            # imm = 0x228
movl $0x6, %esi
movl $0x3, %edx
xorps %xmm0, %xmm0
callq 0x1f5c0
jmp 0x6f02
movq -0x28(%rbp), %rdi
addq $0x258, %rdi            # imm = 0x258
movl $0x3, %esi
movl $0x1, %edx
xorps %xmm0, %xmm0
callq 0x1f5c0
jmp 0x6f21
movq -0x28(%rbp), %rdi
addq $0x270, %rdi            # imm = 0x270
movl $0x6, %esi
movl $0x1, %edx
xorps %xmm0, %xmm0
callq 0x1f5c0
jmp 0x6f40
movq -0x28(%rbp), %rdi
addq $0x288, %rdi            # imm = 0x288
movl $0x6, %edx
xorps %xmm0, %xmm0
movl %edx, %esi
callq 0x1f5c0
jmp 0x6f5c
movq -0x28(%rbp), %rdi
addq $0x2a0, %rdi            # imm = 0x2A0
movl $0x6, %edx
xorps %xmm0, %xmm0
movl %edx, %esi
callq 0x1f5c0
jmp 0x6f78
movq -0x28(%rbp), %rdi
addq $0x2c0, %rdi            # imm = 0x2C0
movl $0x3, %esi
movl $0x1, %edx
xorps %xmm0, %xmm0
callq 0x1f5c0
jmp 0x6f97
movq -0x28(%rbp), %rdi
addq $0x2e0, %rdi            # imm = 0x2E0
movl $0x3, %edx
xorps %xmm0, %xmm0
movl %edx, %esi
callq 0x1f5c0
jmp 0x6fb3
movq -0x28(%rbp), %rdi
addq $0x110, %rdi            # imm = 0x110
movl $0x6, %esi
movl $0x1, %edx
xorps %xmm0, %xmm0
callq 0x1f5c0
jmp 0x6fd2
movq -0x28(%rbp), %rdi
addq $0x1a0, %rdi            # imm = 0x1A0
movl $0x6, %esi
movl $0x1, %edx
xorps %xmm0, %xmm0
callq 0x1f5c0
jmp 0x6ff1
addq $0x90, %rsp
popq %rbp
retq
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0x10(%rbp)
movl %eax, -0x14(%rbp)
jmp 0x714c
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0x10(%rbp)
movl %eax, -0x14(%rbp)
jmp 0x7143
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0x10(%rbp)
movl %eax, -0x14(%rbp)
jmp 0x713a
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0x10(%rbp)
movl %eax, -0x14(%rbp)
jmp 0x7131
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0x10(%rbp)
movl %eax, -0x14(%rbp)
jmp 0x7128
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0x10(%rbp)
movl %eax, -0x14(%rbp)
jmp 0x711f
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0x10(%rbp)
movl %eax, -0x14(%rbp)
jmp 0x7116
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0x10(%rbp)
movl %eax, -0x14(%rbp)
jmp 0x710d
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0x10(%rbp)
movl %eax, -0x14(%rbp)
jmp 0x7104
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0x10(%rbp)
movl %eax, -0x14(%rbp)
jmp 0x70fb
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0x10(%rbp)
movl %eax, -0x14(%rbp)
jmp 0x70f2
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0x10(%rbp)
movl %eax, -0x14(%rbp)
jmp 0x70e9
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0x10(%rbp)
movl %eax, -0x14(%rbp)
jmp 0x70e0
movq -0x88(%rbp), %rdi
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0x10(%rbp)
movl %eax, -0x14(%rbp)
callq 0x1f580
movq -0x80(%rbp), %rdi
callq 0x1f580
movq -0x78(%rbp), %rdi
callq 0x1f580
movq -0x70(%rbp), %rdi
callq 0x1f580
movq -0x68(%rbp), %rdi
callq 0x1f580
movq -0x60(%rbp), %rdi
callq 0x1f580
movq -0x58(%rbp), %rdi
callq 0x1f580
movq -0x50(%rbp), %rdi
callq 0x1f580
movq -0x48(%rbp), %rdi
callq 0x1f580
movq -0x40(%rbp), %rdi
callq 0x1f580
movq -0x38(%rbp), %rdi
callq 0x1f580
movq -0x30(%rbp), %rdi
callq 0x1f580
movq -0x20(%rbp), %rdi
callq 0x1f580
movq -0x28(%rbp), %rdi
callq 0xd790
movq -0x10(%rbp), %rdi
callq 0x33d0
nop