// File: /workspace/github_cpp_makefile_star2_O3/DavidLBruce[P]sim3dof/build/../src/MslTse6.cpp
// Function: MslTse6::MslTse6()
MslTse6::MslTse6() :
pMsl(0),
pExec(0),
pRdr(0),
pSkr(0),
pTgt(0),
pTse6(0),
pTseO(0),
MdlBase()
{
  //
  // Input parameter defaults
  //
  frameTimeStep = 1.0/64.0;
  initFrameTime = 0.0;

  datalinkStep = 1.0 / 64;        // Datalink uplink interval
  nextUpLinkTime = initFrameTime; // Next datalink uplink time
  singerTau = 0.3;                // Kalman process noise tuning parameter
  singerVar = 600.0;              // Kalman process noise tuning parameter

  //
  // Set matrix sizes
  //
  cartRel.Resize(6,1);
  truState.Resize(6,1);
  dlState.Resize(6,1);
  eye.Eye(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);
   
}

// Assembly:
pushq %rbp
movq %rsp, %rbp
pushq %r15
pushq %r14
pushq %r13
pushq %r12
pushq %rbx
subq $0x58, %rsp
movq %rdi, %rbx
callq 0x9fd0
leaq 0x12978(%rip), %rax     # 0x1db60
movq %rax, (%rbx)
leaq 0x120(%rbx), %rdi
xorps %xmm0, %xmm0
movq %rdi, -0x80(%rbp)
movl $0x1, %esi
movl $0x1, %edx
callq 0x13378
leaq 0x1f0(%rbx), %rdi
xorps %xmm0, %xmm0
movq %rdi, -0x78(%rbp)
movl $0x1, %esi
movl $0x1, %edx
callq 0x13378
leaq 0x2c0(%rbx), %rdi
xorps %xmm0, %xmm0
movq %rdi, -0x70(%rbp)
movl $0x1, %esi
movl $0x1, %edx
callq 0x13378
leaq 0x310(%rbx), %rdi
xorps %xmm0, %xmm0
movq %rdi, -0x68(%rbp)
movl $0x1, %esi
movl $0x1, %edx
callq 0x13378
leaq 0x328(%rbx), %rdi
xorps %xmm0, %xmm0
movq %rdi, -0x60(%rbp)
movl $0x1, %esi
movl $0x1, %edx
callq 0x13378
leaq 0x340(%rbx), %rdi
xorps %xmm0, %xmm0
movq %rdi, -0x58(%rbp)
movl $0x1, %esi
movl $0x1, %edx
callq 0x13378
leaq 0x360(%rbx), %rdi
xorps %xmm0, %xmm0
movq %rdi, -0x50(%rbp)
movl $0x1, %esi
movl $0x1, %edx
callq 0x13378
leaq 0x378(%rbx), %rdi
xorps %xmm0, %xmm0
movq %rdi, -0x48(%rbp)
movl $0x1, %esi
movl $0x1, %edx
callq 0x13378
leaq 0x3d0(%rbx), %rdi
xorps %xmm0, %xmm0
movq %rdi, -0x40(%rbp)
movl $0x1, %esi
movl $0x1, %edx
callq 0x13378
leaq 0x3e8(%rbx), %rdi
xorps %xmm0, %xmm0
movq %rdi, -0x38(%rbp)
movl $0x1, %esi
movl $0x1, %edx
callq 0x13378
leaq 0x438(%rbx), %r12
xorps %xmm0, %xmm0
movq %r12, %rdi
movl $0x1, %esi
movl $0x1, %edx
callq 0x13378
leaq 0x450(%rbx), %r13
xorps %xmm0, %xmm0
movq %r13, %rdi
movl $0x1, %esi
movl $0x1, %edx
callq 0x13378
leaq 0x498(%rbx), %r14
xorps %xmm0, %xmm0
movq %r14, %rdi
movl $0x1, %esi
movl $0x1, %edx
callq 0x13378
leaq 0x4b8(%rbx), %r15
xorps %xmm0, %xmm0
movq %r15, %rdi
movl $0x1, %esi
movl $0x1, %edx
callq 0x13378
xorps %xmm0, %xmm0
movups %xmm0, 0x530(%rbx)
movups %xmm0, 0x520(%rbx)
movups %xmm0, 0x510(%rbx)
xorl %eax, %eax
movq %rax, 0x540(%rbx)
movabsq $0x3f90000000000000, %rcx # imm = 0x3F90000000000000
movq %rcx, 0x8(%rbx)
movq %rax, 0xf8(%rbx)
movq %rcx, 0x100(%rbx)
movq %rax, 0x108(%rbx)
movabsq $0x3fd3333333333333, %rax # imm = 0x3FD3333333333333
movq %rax, 0x110(%rbx)
movabsq $0x4082c00000000000, %rax # imm = 0x4082C00000000000
movq %rax, 0x118(%rbx)
xorps %xmm0, %xmm0
movq -0x80(%rbp), %rdi
movl $0x6, %esi
movl $0x1, %edx
callq 0x134d0
xorps %xmm0, %xmm0
movq -0x78(%rbp), %rdi
movl $0x6, %esi
movl $0x1, %edx
callq 0x134d0
xorps %xmm0, %xmm0
movq -0x70(%rbp), %rdi
movl $0x6, %esi
movl $0x1, %edx
callq 0x134d0
movq -0x68(%rbp), %rdi
movl $0x6, %esi
callq 0x1530e
xorps %xmm0, %xmm0
movq -0x60(%rbp), %rdi
movl $0x3, %esi
movl $0x6, %edx
callq 0x134d0
xorps %xmm0, %xmm0
movq -0x58(%rbp), %rdi
movl $0x6, %esi
movl $0x3, %edx
callq 0x134d0
xorps %xmm0, %xmm0
movq -0x50(%rbp), %rdi
movl $0x6, %esi
movl $0x1, %edx
callq 0x134d0
xorps %xmm0, %xmm0
movq -0x48(%rbp), %rdi
movl $0x6, %esi
movl $0x3, %edx
callq 0x134d0
xorps %xmm0, %xmm0
movq -0x40(%rbp), %rdi
movl $0x3, %esi
movl $0x1, %edx
callq 0x134d0
xorps %xmm0, %xmm0
movq -0x38(%rbp), %rdi
movl $0x6, %esi
movl $0x1, %edx
callq 0x134d0
xorps %xmm0, %xmm0
movq %r12, %rdi
movl $0x6, %esi
movl $0x6, %edx
callq 0x134d0
xorps %xmm0, %xmm0
movq %r13, %rdi
movl $0x6, %esi
movl $0x6, %edx
callq 0x134d0
xorps %xmm0, %xmm0
movq %r14, %rdi
movl $0x3, %esi
movl $0x1, %edx
callq 0x134d0
xorps %xmm0, %xmm0
movq %r15, %rdi
movl $0x3, %esi
movl $0x3, %edx
callq 0x134d0
addq $0x58, %rsp
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq
movq %rax, -0x30(%rbp)
jmp 0xb57a
movq %rax, -0x30(%rbp)
jmp 0xb582
movq %rax, -0x30(%rbp)
jmp 0xb58a
movq %rax, -0x30(%rbp)
jmp 0xb592
movq %rax, -0x30(%rbp)
jmp 0xb59b
movq %rax, -0x30(%rbp)
jmp 0xb5a4
movq %rax, -0x30(%rbp)
jmp 0xb5ad
movq %rax, -0x30(%rbp)
jmp 0xb5b6
movq %rax, -0x30(%rbp)
jmp 0xb5bf
movq %rax, -0x30(%rbp)
jmp 0xb5c8
movq %rax, -0x30(%rbp)
jmp 0xb5d1
movq %rax, -0x30(%rbp)
jmp 0xb5da
movq %rax, -0x30(%rbp)
jmp 0xb5e3
movq %rax, -0x30(%rbp)
jmp 0xb5ec
movq %rax, -0x30(%rbp)
movq %r15, %rdi
callq 0x134aa
movq %r14, %rdi
callq 0x134aa
movq %r13, %rdi
callq 0x134aa
movq %r12, %rdi
callq 0x134aa
movq -0x38(%rbp), %rdi
callq 0x134aa
movq -0x40(%rbp), %rdi
callq 0x134aa
movq -0x48(%rbp), %rdi
callq 0x134aa
movq -0x50(%rbp), %rdi
callq 0x134aa
movq -0x58(%rbp), %rdi
callq 0x134aa
movq -0x60(%rbp), %rdi
callq 0x134aa
movq -0x68(%rbp), %rdi
callq 0x134aa
movq -0x70(%rbp), %rdi
callq 0x134aa
movq -0x78(%rbp), %rdi
callq 0x134aa
movq -0x80(%rbp), %rdi
callq 0x134aa
movq %rbx, %rdi
callq 0xa130
movq -0x30(%rbp), %rdi
callq 0x32d0
nop