Start

Calculate

End

Calculate

End

Go back to Start

////Comments

//// q is the number of the sigma point

//// draw the sigma points and their weights using Table 2

//// propagate the points through the filter

//// combining the sigma points to obtain the a priori estimate

//// calculating the a priori covariance matrix

//// Redefine the sigma point and their weight from Table 2 to obtain their a priori measurements

//// combining the sigma points’ measurements to obtain the a priori measurement

//// Calculating the output's error covariance matrix

//// The correction gain

//// Updating the estimate and its covariance matrix

//// Repeat Stages