1. 程式人生 > >035捷聯慣導中三種姿態更新演算法說明

035捷聯慣導中三種姿態更新演算法說明

看到了不同的姿態更新演算法,很迷惑,陷入了誰相對於誰的思考中。翻翻嚴老師部落格,貼出來如下:

原文地址:http://blog.sina.com.cn/s/blog_40edfdc90102v6il.html#cmt_556E9C77-7F000001-41D46DB9-790-8A0


第一種就是將地理系相對於慣性系的角速度在地理系的投影通過k-1時刻的矩陣轉換為地理系相對於慣性系的角速度在載體系的投影,使用k-1時刻的轉換矩陣降低了精度,因為在這個取樣時間段內,該矩陣不能準確刻畫地理系與載體系的轉換關係;

第二種基於第一種轉換矩陣的不準確,將該矩陣由k-1時刻推至k-1/2時刻,相比於第一種精度得到了提高。嚴老師psins140410版本的工具箱中insupdata.m檔案中的更新演算法類似於這一種,程式碼如下:

Cnb1 = q2mat(qmul(ins.qnb,rv2q(phim)));
ins.wnb = ins.wib-(ins.Cnb+Cnb1)'*ins.eth.wnin/2; 
ins.qnb = qmul(ins.qnb, rv2q(ins.wnb*nts)); 

第三種就是之前常用的那種:

qnb = qmul(rv2q(-eth.wnin*nts),qmul(qnb, rv2q(phim)));