Highly Efficient Sigma Point Filter for Spacecraft Attitude and Rate Estimation

Table 4

Pseudocode and computational expense evaluation of the MEKF with closed-form solutions.

	Algorithm	×	+	√
Initialize	𝐱̂0=[𝐚̂^𝑇0,Δ𝐛̂^𝑇0]^𝑇=𝟎6×1, 𝐪̂‌0=[𝟎^𝑇3×1,1]^𝑇, 𝐛̂0=𝟎3×1
	𝐏^𝐴0=𝜀^2𝑎𝐈3, 𝐏^𝐵0=𝜀^2𝑏𝐈3,
	𝐐^𝐴𝑘=𝑇(𝜎^2ARW+(1/3)𝜎^2RRW𝑇^2)𝐈3
	𝐐^𝑘𝐵𝐴=−(1/2)𝜎^2RRW𝑇^2𝐈3, 𝐐^𝐵𝑘=𝜎^2RRW𝑇𝐈3
Propagation	𝝎𝑘−1=𝜔^meas𝑘−1−𝐛̂𝑘−1, 𝜔𝑘−1=|𝝎𝑘−1|, 𝜙𝑘−1=𝜔𝑘−1𝑇	6	5	1
	𝐪̂‌^𝝎𝑘−1=[8𝝓^𝑇𝑘−1,(16−𝜙^2𝑘−1)]^𝑇/(16+𝜙^2𝑘−1)	6	2
	𝐧̂=𝝎𝑘−1/𝜔𝑘−1	3
	Φ𝑘−1=𝐈3−sin𝜙𝑘−1[𝐧̂×]+(1−cos𝜙𝑘−1)[𝐧̂×]^2≈𝐈3−(𝜙𝑘−1−(𝜙^3𝑘−1/6))[𝐧̂×]+(𝜙^2𝑘−1/2)[𝐧̂×]^2	49	30
	Ψ𝑘−1=+(𝑇((/2𝜙)𝐈3𝑘−1−((1−cos𝜙𝑘−1)/𝜔𝑘−1)[𝐧̂𝑘−1×]−sin𝜙𝑘−1)/𝜔𝑘−1)[𝐧̂𝑘−1×]^2≈(𝑇/2)𝐈3−(𝜙^2𝑘−1/2𝜔𝑘−1)[𝐧̂𝑘−1×]+(𝜙^3𝑘−1/6𝜔𝑘−1)[𝐧̂𝑘−1×]^2	51	30
	𝐏^𝐴𝑘/𝑘−1=Φ𝑘−1𝐏^𝐴𝑘−1Φ^𝑇𝑘−1+Ψ𝑘−1𝐏^𝐵𝑘−1Ψ^𝑇𝑘−1	108	81
	+Ψ𝑘−1𝐏^𝐵𝐴𝑘−1Φ^𝑇𝑘−1+(Ψ𝑘−1𝐏^𝐵𝐴𝑘−1Φ^𝑇𝑘−1)^𝑇+𝐐^𝐴𝑘	54	57
	𝐏^𝐵𝐴𝑘/𝑘−1=𝐏^𝐵𝐴𝑘−1Φ^𝑇𝑘−1+𝐏^𝐵𝑘−1Ψ^𝑇𝑘−1+𝐐^𝑘𝐵𝐴	54	48
	𝐏^𝐵𝑘/𝑘−1=𝐏^𝐵𝑘−1+𝐐^𝐵𝑘		3
	𝐪̂‌𝑘/𝑘−1=𝐪̂𝑘−1⊗𝐪̂^𝝎𝑘−1, 𝐪̂‌𝑘/𝑘−1=𝐪̂𝑘/𝑘−1/‖𝐪̂𝑘/𝑘−1‖	22	15	1
	𝐛̂𝑘/𝑘−1=𝐛̂𝑘−1
		353	271	2
Measurement update	𝐏^𝑍𝑘/𝑘−1=𝐇𝑘𝐏^𝑋𝑘/𝑘−1𝐇^𝑇𝑘+𝐑𝑘=𝐏^𝐴𝑘/𝑘−1+𝐑𝑘		3
	𝐏^𝐴𝑍𝑘/𝑘−1=𝐏^𝐴𝑘/𝑘−1𝐇^𝑇𝑘=𝐏^𝐴𝑘/𝑘−1
	𝐏^𝐵𝑍𝑘/𝑘−1=𝐏^𝐵𝐴𝑘/𝑘−1𝐇^𝑇𝑘=𝐏^𝐵𝐴𝑘/𝑘−1
	𝐊^𝑘𝐴𝑍=𝐏^𝐴𝑘/𝑘−1(𝐏^𝑍𝑘/𝑘−1)^−1, 𝐊^𝑘𝐵𝑍=𝐏^𝐵𝐴𝑘/𝑘−1(𝐏^𝑍𝑘/𝑘−1)^−1	114	58
	𝛿𝐪^meas𝑘=𝐪̂^−1𝑘/𝑘−1⊗𝐪^meas𝑘	16	12
	𝑧^meas𝑘=4𝐪^meas𝑘/(1+𝑞4^meas𝑘), 𝜸^meas𝑘=𝑧^meas𝑘̂	5	4
	𝐚̂𝑘=𝐊^𝑘𝐴𝑍𝜸^meas𝑘, Δ𝐛̂𝑘=𝐊^𝑘𝐵𝑍𝜸^meas𝑘	18	9
	𝐏^𝐴𝑘=𝐏^𝐴𝑘/𝑘−1−𝐊^𝑘𝐴𝑍(𝐏^𝐴𝑍𝑘/𝑘−1)^𝑇	27	27
	𝐏^𝑘𝐵𝐴=𝐏^𝐵𝐴𝑘/𝑘−1−𝐊^𝑘𝐵𝑍(𝐏^𝐴𝑍𝑘/𝑘−1)^𝑇	27	27
	𝐏^𝐵𝑘=𝐏^𝐵𝑘/𝑘−1−𝐊^𝐵𝑍𝑘/𝑘−1(𝐏^𝐵𝑍𝑘/𝑘−1)^𝑇	27	27
	𝐪̂‌𝑘=𝐪̂𝑘/𝑘−1⊗𝛿𝐪̂(𝐚̂𝑘), 𝐪̂‌𝑘=𝐪̂𝑘/‖𝐪̂𝑘‖	22	15	1
	𝐛̂𝑘=𝐛̂𝑘/𝑘−1+Δ𝐛̂𝑘		3
		256	185	1