Highly Efficient Sigma Point Filter for Spacecraft Attitude and Rate Estimation

Table 5

Pseudocode and computational expense evaluation of the MGSPF algorithm.

	Algorithm			×	+	√
Initialize	𝐱̂0=[𝐚̂^𝑇0,Δ𝐛̂^𝑇0]^𝑇=𝟎6×1, 𝐪̂‌0=[𝟎^𝑇3×1,1]^𝑇, 𝐛̂0=𝟎3×1
	𝐏^𝐴0=𝜀^2𝑎𝐈3, 𝐏^0𝐵𝐴=𝜀𝑎𝜀𝑏𝐈3    m = 4
	𝐒^𝑄𝐴𝑘−1=𝑇((6𝜎^2ARW−𝑇^2𝜎^2RRW)/12)𝐈3𝐒^𝑄𝐵𝑘−1=(𝜎RRW√𝑇/2)𝐈3
Propagation	𝐒^𝐴𝑘−1=𝐏^𝐴𝑘−1, 𝐒^𝐵𝐴𝑘−1=𝐏^𝐵𝐴𝑘−1/(𝐒^𝐴𝑘−1)^𝑇			25	13	3
	𝑎𝑘−1=(𝐒^𝐴𝑘−1+𝐒^𝑄𝐴𝑘−1)𝖀^𝐺𝑆, 𝕭𝑘−1=(𝐒^𝐵𝐴𝑘−1−𝐒^𝑄𝐵𝑘−1)𝖀^𝐺𝑆			0	54
	𝝎𝑘−1=𝜔^meas𝑘−1−𝐛̂𝑘−1, 𝜔𝑘−1=|𝝎𝑘−1|, 𝜙𝑘−1=𝜔𝑘−1𝑇			6	5	1
	𝐪̂‌^𝝎𝑘−1=[8𝝓^𝑇𝑘−1,(16−𝜙^2𝑘−1)]^𝑇/(16+𝜙^2𝑘−1)			6	2
	For 𝐢=𝟏∶𝐦,  𝝓^𝜷(𝑖)𝑘−1=(𝝎𝑘−1−𝜷^(𝑖)𝑘−1)𝑇			3m	3m
	𝐪^𝜷(𝑖)𝑘−1=[8𝝓^𝜷(𝑖)𝑇𝑘−1,(16−𝜙^𝜷(𝑖)2𝑘−1)]^𝑇/(16+𝜙^𝜷(𝑖)2𝑘−1)			6m	2m
	𝐪^𝜶(𝑖)𝑘−1=[8𝜶^(𝑖)𝑇𝑘−1,(16−𝛼^(𝑖)2𝑘−1)]^𝑇/(16+𝛼^(𝑖)2𝑘−1)			6m	2m
	𝐪^𝜶(𝑖)𝑘/𝑘−1=(𝐪̂^𝜔𝑘−1)^−1⊗𝐪^𝜶(𝑖)𝑘−1⊗𝐪^𝜷(𝑖)𝑘−1			32m	24m
	𝜶^(𝑖)𝑘/𝑘−1=4𝐪^𝜶(𝑖)𝑘/𝑘−1/(1+𝑞4𝜶(𝑖)𝑘/𝑘−1) end			5m	1m
	𝕭𝑘/𝑘−1=𝕭𝑘−1, 𝖅𝑘/𝑘−1=𝑎𝑘/𝑘−1
	𝐚̂𝑘/𝑘−1=(1/𝑚)𝑎𝑘/𝑘−1⋅𝟏1×𝑚1×𝑚, Δ𝐛̂𝑘/𝑘−1=Δ𝐛̂𝑘−1=𝟎3×1			3	3(𝑚−1)
	𝑎𝑘/𝑘−1=𝑎𝑘/𝑘−1−𝟏1×𝑚∘𝐚̂𝑘/𝑘−1				3m
	𝐏^𝐴𝑘/𝑘−1=(1/𝑚)𝑎𝑘/𝑘−1(𝑎𝑘/𝑘−1)^𝑇		6+6𝑚		6(𝑚−1)
	𝐏^𝐵𝐴𝑘/𝑘−1=(1/𝑚)𝕭𝑘/𝑘−1(𝑎𝑘/𝑘−1)^𝑇		9+9𝑚		9(𝑚−1)
	𝐪̂‌𝑘/𝑘−1=𝐪̂𝑘−1⊗𝐪̂^𝝎𝑘−1, 𝐪̂‌𝑘/𝑘−1=𝐪̂𝑘/𝑘−1/‖𝐪̂𝑘/𝑘−1‖			22	15	1
	𝐛̂𝑘/𝑘−1=𝐛̂𝑘−1
		For 𝑚=4		351	285	5
Measurement update	𝑧̂𝑘/𝑘−1=(1/𝑚)𝖅𝑘/𝑘−1•1𝑚×1			3	3(𝑚−1)
	𝖅𝑘/𝑘−1=𝖅𝑘/𝑘−1−𝟏1×𝑚∘𝑧̂𝑘/𝑘−1				3m
	𝐏^𝑍𝑘/𝑘−1=(1/𝑚)𝖅𝑘/𝑘−1𝖅𝑇𝑘/𝑘−1+𝐑𝑘		6+6𝑚		9(𝑚−1)+3
	𝐏^𝐴𝑍𝑘/𝑘−1=(1/𝑚)𝑎𝑘/𝑘−1𝖅𝑇𝑘/𝑘−1		9+9𝑚		9(𝑚−1)
	𝐏^𝐵𝑍𝑘/𝑘−1=(1/𝑚)𝕭𝑘/𝑘−1𝖅𝑇𝑘/𝑘−1		9+9𝑚		9(𝑚−1)
	𝐊^𝑘𝐴𝑍=𝐏^𝐴𝑘/𝑘−1(𝐏^𝑍𝑘/𝑘−1)^−1, 𝐊^𝑘𝐵𝑍=𝐏^𝐵𝐴𝑘/𝑘−1(𝐏^𝑍𝑘/𝑘−1)^−1			114	58
	𝛿𝐪^meas𝑘=𝐪^−1𝑘/𝑘−1⊗𝐪^meas𝑘			16	12
	𝑧^meas𝑘=4𝐪^meas𝑘/(1+𝑞4^meas𝑘), 𝜸^meas𝑘=𝑧^meas𝑘̂−𝑧̂𝑘/𝑘−1			5	4
	𝐚̂𝑘=𝐊^𝑘𝐴𝑍𝜸^meas𝑘, Δ𝐛̂𝑘=𝐊^𝑘𝐵𝑍𝜸^meas𝑘			18	12
	𝐏^𝐴𝑘=𝐏^𝐴𝑘/𝑘−1−𝐊^𝑘𝐴𝑍(𝐏^𝐴𝑍𝑘/𝑘−1)^𝑇			27	27
	𝐏^𝑘𝐵𝐴=𝐏^𝐵𝐴𝑘/𝑘−1−𝐊^𝑘𝐵𝑍(𝐏^𝐴𝑍𝑘/𝑘−1)^𝑇			27	27
	𝐏^𝐵𝑘=𝐏^𝐵𝑘/𝑘−1−𝐊^𝐵𝑍𝑘/𝑘−1(𝐏^𝐵𝑍𝑘/𝑘−1)^𝑇			27	27
	𝐪̂‌𝑘=𝐪̂𝑘/𝑘−1⊗𝛿𝐪(𝐚̂𝑘), 𝐪̂‌𝑘=𝐪̂𝑘/‖𝐪̂𝑘‖			22	15	1
	𝐛̂𝑘=𝐛̂𝑘/𝑘−1+Δ𝐛̂𝑘				3
		For 𝑚=4		379	278	1