// File: 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
subq $0x90, %rsp
movq %rdi, -0x8(%rbp)
movq -0x8(%rbp), %rdi
movq %rdi, -0x28(%rbp)
callq 0xd640
movq -0x28(%rbp), %rdi
leaq 0x1d349(%rip), %rax     # 0x2eb10
movq %rax, (%rdi)
addq $0x120, %rdi            # imm = 0x120
movq %rdi, -0x20(%rbp)
movl $0x1, %edx
xorps %xmm0, %xmm0
movl %edx, %esi
callq 0x1f2e0
jmp 0x117e6
movq -0x28(%rbp), %rdi
addq $0x1f0, %rdi            # imm = 0x1F0
movq %rdi, -0x30(%rbp)
movl $0x1, %edx
xorps %xmm0, %xmm0
movl %edx, %esi
callq 0x1f2e0
jmp 0x11806
movq -0x28(%rbp), %rdi
addq $0x2c0, %rdi            # imm = 0x2C0
movq %rdi, -0x38(%rbp)
movl $0x1, %edx
xorps %xmm0, %xmm0
movl %edx, %esi
callq 0x1f2e0
jmp 0x11826
movq -0x28(%rbp), %rdi
addq $0x310, %rdi            # imm = 0x310
movq %rdi, -0x40(%rbp)
movl $0x1, %edx
xorps %xmm0, %xmm0
movl %edx, %esi
callq 0x1f2e0
jmp 0x11846
movq -0x28(%rbp), %rdi
addq $0x328, %rdi            # imm = 0x328
movq %rdi, -0x48(%rbp)
movl $0x1, %edx
xorps %xmm0, %xmm0
movl %edx, %esi
callq 0x1f2e0
jmp 0x11866
movq -0x28(%rbp), %rdi
addq $0x340, %rdi            # imm = 0x340
movq %rdi, -0x50(%rbp)
movl $0x1, %edx
xorps %xmm0, %xmm0
movl %edx, %esi
callq 0x1f2e0
jmp 0x11886
movq -0x28(%rbp), %rdi
addq $0x360, %rdi            # imm = 0x360
movq %rdi, -0x58(%rbp)
movl $0x1, %edx
xorps %xmm0, %xmm0
movl %edx, %esi
callq 0x1f2e0
jmp 0x118a6
movq -0x28(%rbp), %rdi
addq $0x378, %rdi            # imm = 0x378
movq %rdi, -0x60(%rbp)
movl $0x1, %edx
xorps %xmm0, %xmm0
movl %edx, %esi
callq 0x1f2e0
jmp 0x118c6
movq -0x28(%rbp), %rdi
addq $0x3d0, %rdi            # imm = 0x3D0
movq %rdi, -0x68(%rbp)
movl $0x1, %edx
xorps %xmm0, %xmm0
movl %edx, %esi
callq 0x1f2e0
jmp 0x118e6
movq -0x28(%rbp), %rdi
addq $0x3e8, %rdi            # imm = 0x3E8
movq %rdi, -0x70(%rbp)
movl $0x1, %edx
xorps %xmm0, %xmm0
movl %edx, %esi
callq 0x1f2e0
jmp 0x11906
movq -0x28(%rbp), %rdi
addq $0x438, %rdi            # imm = 0x438
movq %rdi, -0x78(%rbp)
movl $0x1, %edx
xorps %xmm0, %xmm0
movl %edx, %esi
callq 0x1f2e0
jmp 0x11926
movq -0x28(%rbp), %rdi
addq $0x450, %rdi            # imm = 0x450
movq %rdi, -0x80(%rbp)
movl $0x1, %edx
xorps %xmm0, %xmm0
movl %edx, %esi
callq 0x1f2e0
jmp 0x11946
movq -0x28(%rbp), %rdi
addq $0x498, %rdi            # imm = 0x498
movq %rdi, -0x88(%rbp)
movl $0x1, %edx
xorps %xmm0, %xmm0
movl %edx, %esi
callq 0x1f2e0
jmp 0x11969
movq -0x28(%rbp), %rdi
addq $0x4b8, %rdi            # imm = 0x4B8
movq %rdi, -0x90(%rbp)
movl $0x1, %edx
xorps %xmm0, %xmm0
movl %edx, %esi
callq 0x1f2e0
jmp 0x1198c
movq -0x28(%rbp), %rdi
movq $0x0, 0x510(%rdi)
movq $0x0, 0x518(%rdi)
movq $0x0, 0x520(%rdi)
movq $0x0, 0x528(%rdi)
movq $0x0, 0x530(%rdi)
movq $0x0, 0x538(%rdi)
movq $0x0, 0x540(%rdi)
movabsq $0x3f90000000000000, %rax # imm = 0x3F90000000000000
movq %rax, 0x8(%rdi)
movq $0x0, 0xf8(%rdi)
movq %rax, 0x100(%rdi)
movsd 0xf8(%rdi), %xmm0
movsd %xmm0, 0x108(%rdi)
movabsq $0x3fd3333333333333, %rax # imm = 0x3FD3333333333333
movq %rax, 0x110(%rdi)
movabsq $0x4082c00000000000, %rax # imm = 0x4082C00000000000
movq %rax, 0x118(%rdi)
addq $0x120, %rdi            # imm = 0x120
movl $0x6, %esi
movl $0x1, %edx
xorps %xmm0, %xmm0
callq 0x1f5c0
jmp 0x11a4a
movq -0x28(%rbp), %rdi
addq $0x1f0, %rdi            # imm = 0x1F0
movl $0x6, %esi
movl $0x1, %edx
xorps %xmm0, %xmm0
callq 0x1f5c0
jmp 0x11a69
movq -0x28(%rbp), %rdi
addq $0x2c0, %rdi            # imm = 0x2C0
movl $0x6, %esi
movl $0x1, %edx
xorps %xmm0, %xmm0
callq 0x1f5c0
jmp 0x11a88
movq -0x28(%rbp), %rdi
addq $0x310, %rdi            # imm = 0x310
movl $0x6, %esi
callq 0x22b00
jmp 0x11a9f
movq -0x28(%rbp), %rdi
addq $0x328, %rdi            # imm = 0x328
movl $0x3, %esi
movl $0x6, %edx
xorps %xmm0, %xmm0
callq 0x1f5c0
jmp 0x11abe
movq -0x28(%rbp), %rdi
addq $0x340, %rdi            # imm = 0x340
movl $0x6, %esi
movl $0x3, %edx
xorps %xmm0, %xmm0
callq 0x1f5c0
jmp 0x11add
movq -0x28(%rbp), %rdi
addq $0x360, %rdi            # imm = 0x360
movl $0x6, %esi
movl $0x1, %edx
xorps %xmm0, %xmm0
callq 0x1f5c0
jmp 0x11afc
movq -0x28(%rbp), %rdi
addq $0x378, %rdi            # imm = 0x378
movl $0x6, %esi
movl $0x3, %edx
xorps %xmm0, %xmm0
callq 0x1f5c0
jmp 0x11b1b
movq -0x28(%rbp), %rdi
addq $0x3d0, %rdi            # imm = 0x3D0
movl $0x3, %esi
movl $0x1, %edx
xorps %xmm0, %xmm0
callq 0x1f5c0
jmp 0x11b3a
movq -0x28(%rbp), %rdi
addq $0x3e8, %rdi            # imm = 0x3E8
movl $0x6, %esi
movl $0x1, %edx
xorps %xmm0, %xmm0
callq 0x1f5c0
jmp 0x11b59
movq -0x28(%rbp), %rdi
addq $0x438, %rdi            # imm = 0x438
movl $0x6, %edx
xorps %xmm0, %xmm0
movl %edx, %esi
callq 0x1f5c0
jmp 0x11b75
movq -0x28(%rbp), %rdi
addq $0x450, %rdi            # imm = 0x450
movl $0x6, %edx
xorps %xmm0, %xmm0
movl %edx, %esi
callq 0x1f5c0
jmp 0x11b91
movq -0x28(%rbp), %rdi
addq $0x498, %rdi            # imm = 0x498
movl $0x3, %esi
movl $0x1, %edx
xorps %xmm0, %xmm0
callq 0x1f5c0
jmp 0x11bb0
movq -0x28(%rbp), %rdi
addq $0x4b8, %rdi            # imm = 0x4B8
movl $0x3, %edx
xorps %xmm0, %xmm0
movl %edx, %esi
callq 0x1f5c0
jmp 0x11bcc
addq $0x90, %rsp
popq %rbp
retq
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0x10(%rbp)
movl %eax, -0x14(%rbp)
jmp 0x11d44
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0x10(%rbp)
movl %eax, -0x14(%rbp)
jmp 0x11d3b
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0x10(%rbp)
movl %eax, -0x14(%rbp)
jmp 0x11d32
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0x10(%rbp)
movl %eax, -0x14(%rbp)
jmp 0x11d29
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0x10(%rbp)
movl %eax, -0x14(%rbp)
jmp 0x11d20
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0x10(%rbp)
movl %eax, -0x14(%rbp)
jmp 0x11d17
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0x10(%rbp)
movl %eax, -0x14(%rbp)
jmp 0x11d0e
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0x10(%rbp)
movl %eax, -0x14(%rbp)
jmp 0x11d05
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0x10(%rbp)
movl %eax, -0x14(%rbp)
jmp 0x11cfc
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0x10(%rbp)
movl %eax, -0x14(%rbp)
jmp 0x11cf3
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0x10(%rbp)
movl %eax, -0x14(%rbp)
jmp 0x11cea
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0x10(%rbp)
movl %eax, -0x14(%rbp)
jmp 0x11ce1
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0x10(%rbp)
movl %eax, -0x14(%rbp)
jmp 0x11cd8
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0x10(%rbp)
movl %eax, -0x14(%rbp)
jmp 0x11ccc
movq -0x90(%rbp), %rdi
movq %rax, %rcx
movl %edx, %eax
movq %rcx, -0x10(%rbp)
movl %eax, -0x14(%rbp)
callq 0x1f580
movq -0x88(%rbp), %rdi
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
nopw %cs:(%rax,%rax)