Persamaan ( 6 ) dan ( 7 ) mengungkapkan hubungan geometris yang benar . Kita tahu bahwa jika nilai-nilai yang benar terpasang di untuk semua nilai dalam Gij , maka mereka memang akan sama dengan nol . Kita tahu semua nilai kecuali enam variabel posisi dan orientasi yang mewakili . Jadi apa yang dapat persamaan ( 6 ) dan ( 7 ) memberitahu kami tentang orang hilang enam variabel

Satu pendekatan adalah untuk pertama membuat dugaan awal posisi dan orientasi , kemudian menulis ulang persamaan ( 6 ) dan ( 7 ) sedemikian rupa sehingga ketika menebak ini terpasang , memberikan faktor koreksi bahwa kita dapat menambah enam tebakan kita untuk mendapatkan menebak lebih akurat . Kita kemudian dapat pasang di duga baru ini dan menghitung faktor koreksi baru . Siklus ini berulang sampai faktor koreksi menjadi begitu kecil bahwa kita berkumpul untuk solusi.

Persyaratan membutuhkan menebak awal posisi pengguna dan orientasi sebenarnya cukup masuk akal dalam aplikasi tertentu kami . Kami biasanya memiliki dugaan awal yang sangat baik tersedia : posisi terakhir yang diketahui dan orientasi . Karena sistem kami dapat melacak kepala pengguna dengan harga antara 20 dan 100 Hz , posisi terakhir yang diketahui dan orientasi akan hampir pasti akan sangat dekat dengan posisi saat ini dan orientasi . Satu-satunya saat ketika kita mengalami kesulitan adalah ketika kita tidak tahu di mana pengguna , seperti pada sistem startup atau saat pengguna mengarahkan kepalanya sehingga terlalu sedikit unit fotodioda dapat melihat langit-langit . Kami membahas pulih dari situasi merosot seperti dalam Bagian 3.5 .

Untuk mendapatkan faktor koreksi ini , kita melinearisasi persamaan ( 6 ) dan ( 7 ) dengan menggunakan teorema Taylor untuk mendapatkan pendekatan orde pertama linear:

di mana : PO singkatan menebak dari posisi pengguna dan orientasi , Gij 1 PO adalah nilai Gij diberikan PO ,

adalah nilai dari derivatif parsial diberikan PO

Setiap LED bahwa sistem kami melihat menghasilkan dua persamaan tersebut . Mengatakan bahwa kita melihat N LED . Kemudian kita dapat menulis 2N persamaan yang dihasilkan dalam bentuk matriks sebagai berikut :

dimana vektor dan matriks didefinisikan sebagai berikut :

D adalah apa yang kita ingin mencari , karena memegang faktor koreksi yang akan kita gunakan untuk memodifikasi menebak kita tentang posisi dan orientasi dari kepala . Jadi bagaimana kita menulis ulang persamaan ( 8 ) untuk mengekstrak D ?

Kita sekarang daftar beberapa cara untuk memulihkan D :

Kita perlu melihat setidaknya tiga LED untuk menghasilkan enam persamaan , atau sistem kami akan underdetermined . Jika kita melihat persis tiga LED , maka ∂ G menjadi matriks 6x6 dan kami bisa membalikkan untuk mendapatkan

Tapi jika kita melihat lebih dari tiga LED , sistem kami menjadi overdetermined dan kita dapat menggunakan sedikitnya pendekatan kotak dengan mengambil pseudo- inverse dari ∂ G :

Kita juga bisa menggunakan kuadrat terkecil tertimbang dengan memperkenalkan P matriks baru yang beratnya nilai-nilai dari setiap pengukuran LED berbeda . LED yang jauh dari kepala kita menghasilkan pengukuran yang lebih berisik dari LED yang dekat , karena rasio sinyal / noise berkurang dan karena keterbatasan resolusi foto dioda kami . Meskipun saat ini kami tidak melakukan hal ini , kita bisa menciptakan P untuk mengurangi kontribusi LED jauh dari kepala pengguna . Rumus untuk mengekstrak D kemudian menjadi :

di mana P adalah matriks diagonal 2Nx2N dalam bentuk:

dan p1 ke pN adalah bobot untuk setiap pengukuran LED.

Akhirnya, kita dapat menggunakan dekomposisi nilai singular untuk mengekstrak D. Ini adalah saat ini metode kami pilihan karena menangani sakit-AC matriks ∂ G lebih baik daripada metode lain bisa. Untuk Deskripsi tentang bagaimana metode ini bekerja, silakan lihat Bagian 6.2.

Kita sekarang dapat menguraikan loop utama dari collinearity di psuedocode:

Beberapa catatan implementasi:

• Saat ini kami menetapkan iteration_limit sampai 10, karena collinearity cenderung untuk berkumpul dengan cepat ketika ia dapat menemukan solusi (lihat Bagian 3.3).

• Apa artinya untuk D untuk menjadi "cukup kecil?" Saat ini kami menggunakan kriteria sebagai berikut:

karena batas-batas ini memberikan akurasi yang memadai sementara masih memungkinkan konvergensi dalam jumlah yang wajar dari iterasi