| <Propagation Step> |
| - Attitude |
| ωB/NB=(ω˜B/IB−bg−ηv)−CNB(q)ωN/IN |
| q˙=12Ξ(q)ωB/NB |
| - Position and velocity |
| φ˙=VN(Rφ+h),λ˙=VE(Rλ+h)cosφ,h˙=−vD |
| v˙N=−[ VE(Rλ+h)cosφ+2ωe ]vEsinφ+vNvDRφ+h+aN |
| v˙E=[ VE(Rλ+h)cosφ+2ωe ]vNsinφ+vEvDRλ+h+2ωevDcosφ+aE |
| v˙D=−vE2Rλ+h−vN2Rφ+h−2ωevEcosφ+g+aD |
| - Covariance matrix |
| P˙(t)=F[ x^(t),t ]P(t)+P(t)FT[ x^(t),t ]+G(t)Q(t)GT(t) |
| <Update Step From Each Sensor> |
| - Sensitivity matrix of star sensor |
| HST=[ I3×3,Hδp,ST,03×9 ] |
| - Sensitivity matrix of altimeter |
| Halt=[ 01×5,1,01×9 ] |
| - Sensitivity matrix of celestial navigation |
| - FOR i = 1:N |
| HiCNS_ tightly=[ 0N×3,Hδφ,Hδλ,0N×10 ] |
| END FOR |
| H=[ HST,Halt,HCNS ]T |
| - Gain calculation |
| Kk=Pk−HT(x^k−)[ HPk−HT(x^k−)+R ]−1 |
| Pk+=[ I−KkHT(x^k−) ]Pk− |
| - Final update |
| Δx^k+=Kk[ z˜−H(x^k−) ] |
| x^k+=x^k−+Δx^k+ |