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

  initOmega = 4.0;
  initPosY = 0.0;
  initPosYsigScale = 1.4;
  initVelY = 0.0;
  initVelYsig = 5.0;
  initWsig = 2.0;
//  procNoise = 0.1e-3;
  procNoise = 0.0128;

  //
  // Set matrix sizes.
  //
  eye.Eye(3);
  HH.Resize(1,3);
  HT.Resize(3,1);
  innovation.Resize(3,1);
  KK.Resize(3,1);
  phi.Resize(3,3);
  PP.Resize(3,3);
  QQ.Resize(3,3);
  stateVec.Resize(3,1);

}

// Assembly:
pushq %rbp
movq %rsp, %rbp
subq $0x70, %rsp
movq %rdi, -0x8(%rbp)
movq -0x8(%rbp), %rdi
movq %rdi, -0x28(%rbp)
callq 0xd640
movq -0x28(%rbp), %rdi
leaq 0x2a47c(%rip), %rax     # 0x2e910
movq %rax, (%rdi)
addq $0x158, %rdi            # imm = 0x158
movq %rdi, -0x20(%rbp)
movl $0x1, %edx
xorps %xmm0, %xmm0
movl %edx, %esi
callq 0x1f2e0
jmp 0x44b3
movq -0x28(%rbp), %rdi
addq $0x170, %rdi            # imm = 0x170
movq %rdi, -0x30(%rbp)
movl $0x1, %edx
xorps %xmm0, %xmm0
movl %edx, %esi
callq 0x1f2e0
jmp 0x44d3
movq -0x28(%rbp), %rdi
addq $0x188, %rdi            # imm = 0x188
movq %rdi, -0x38(%rbp)
movl $0x1, %edx
xorps %xmm0, %xmm0
movl %edx, %esi
callq 0x1f2e0
jmp 0x44f3
movq -0x28(%rbp), %rdi
addq $0x1a8, %rdi            # imm = 0x1A8
movq %rdi, -0x40(%rbp)
movl $0x1, %edx
xorps %xmm0, %xmm0
movl %edx, %esi
callq 0x1f2e0
jmp 0x4513
movq -0x28(%rbp), %rdi
addq $0x1c0, %rdi            # imm = 0x1C0
movq %rdi, -0x48(%rbp)
movl $0x1, %edx
xorps %xmm0, %xmm0
movl %edx, %esi
callq 0x1f2e0
jmp 0x4533
movq -0x28(%rbp), %rdi
addq $0x1e0, %rdi            # imm = 0x1E0
movq %rdi, -0x50(%rbp)
movl $0x1, %edx
xorps %xmm0, %xmm0
movl %edx, %esi
callq 0x1f2e0
jmp 0x4553
movq -0x28(%rbp), %rdi
addq $0x1f8, %rdi            # imm = 0x1F8
movq %rdi, -0x58(%rbp)
movl $0x1, %edx
xorps %xmm0, %xmm0
movl %edx, %esi
callq 0x1f2e0
jmp 0x4573
movq -0x28(%rbp), %rdi
addq $0x228, %rdi            # imm = 0x228
movq %rdi, -0x60(%rbp)
movl $0x1, %edx
xorps %xmm0, %xmm0
movl %edx, %esi
callq 0x1f2e0
jmp 0x4593
movq -0x28(%rbp), %rdi
addq $0x248, %rdi            # imm = 0x248
movq %rdi, -0x68(%rbp)
movl $0x1, %edx
xorps %xmm0, %xmm0
movl %edx, %esi
callq 0x1f2e0
jmp 0x45b3
movq -0x28(%rbp), %rdi
movabsq $0x3f90000000000000, %rax # imm = 0x3F90000000000000
movq %rax, 0x8(%rdi)
movq $0x0, 0xf8(%rdi)
movabsq $0x4010000000000000, %rax # imm = 0x4010000000000000
movq %rax, 0x100(%rdi)
movq $0x0, 0x108(%rdi)
movabsq $0x3ff6666666666666, %rax # imm = 0x3FF6666666666666
movq %rax, 0x110(%rdi)
movq $0x0, 0x118(%rdi)
movabsq $0x4014000000000000, %rax # imm = 0x4014000000000000
movq %rax, 0x120(%rdi)
movabsq $0x4000000000000000, %rax # imm = 0x4000000000000000
movq %rax, 0x128(%rdi)
movabsq $0x3f8a36e2eb1c432d, %rax # imm = 0x3F8A36E2EB1C432D
movq %rax, 0x130(%rdi)
addq $0x158, %rdi            # imm = 0x158
movl $0x3, %esi
callq 0x22b00
jmp 0x464e
movq -0x28(%rbp), %rdi
addq $0x170, %rdi            # imm = 0x170
movl $0x1, %esi
movl $0x3, %edx
xorps %xmm0, %xmm0
callq 0x1f5c0
jmp 0x466d
movq -0x28(%rbp), %rdi
addq $0x188, %rdi            # imm = 0x188
movl $0x3, %esi
movl $0x1, %edx
xorps %xmm0, %xmm0
callq 0x1f5c0
jmp 0x468c
movq -0x28(%rbp), %rdi
addq $0x1a8, %rdi            # imm = 0x1A8
movl $0x3, %esi
movl $0x1, %edx
xorps %xmm0, %xmm0
callq 0x1f5c0
jmp 0x46ab
movq -0x28(%rbp), %rdi
addq $0x1c0, %rdi            # imm = 0x1C0
movl $0x3, %esi
movl $0x1, %edx
xorps %xmm0, %xmm0
callq 0x1f5c0
jmp 0x46ca
movq -0x28(%rbp), %rdi
addq $0x1e0, %rdi            # imm = 0x1E0
movl $0x3, %edx
xorps %xmm0, %xmm0
movl %edx, %esi
callq 0x1f5c0
jmp 0x46e6
movq -0x28(%rbp), %rdi
addq $0x1f8, %rdi            # imm = 0x1F8
movl $0x3, %edx
xorps %xmm0, %xmm0
movl %edx, %esi
callq 0x1f5c0
jmp 0x4702
movq -0x28(%rbp), %rdi
addq $0x228, %rdi            # imm = 0x228
movl $0x3, %edx
xorps %xmm0, %xmm0
movl %edx, %esi
callq 0x1f5c0
jmp 0x471e
movq -0x28(%rbp), %rdi
addq $0x248, %rdi            # imm = 0x248
movl $0x3, %esi
movl $0x1, %edx
xorps %xmm0, %xmm0
callq 0x1f5c0
jmp 0x473d
addq $0x70, %rsp
popq %rbp
retq
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0x10(%rbp)
movl %eax, -0x14(%rbp)
jmp 0x482a
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0x10(%rbp)
movl %eax, -0x14(%rbp)
jmp 0x4821
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0x10(%rbp)
movl %eax, -0x14(%rbp)
jmp 0x4818
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0x10(%rbp)
movl %eax, -0x14(%rbp)
jmp 0x480f
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0x10(%rbp)
movl %eax, -0x14(%rbp)
jmp 0x4806
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0x10(%rbp)
movl %eax, -0x14(%rbp)
jmp 0x47fd
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0x10(%rbp)
movl %eax, -0x14(%rbp)
jmp 0x47f4
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0x10(%rbp)
movl %eax, -0x14(%rbp)
jmp 0x47eb
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0x10(%rbp)
movl %eax, -0x14(%rbp)
jmp 0x47e2
movq -0x68(%rbp), %rdi
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0x10(%rbp)
movl %eax, -0x14(%rbp)
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
nopl (%rax)