P0=[ ...
    0.0021
    0.0138
    0.0150
   -0.0087];
                                                                                
P = [ ...
    0.9866    0.0183   -0.0006    0.0054
    0.0101    0.9430    0.0193    0.0025
   -0.0309    0.0213    0.9643    0.0040
    0.0032    0.0032    0.0089    0.9977];
                                                                                
Q = [ ...
   -0.0114         0         0         0
   -0.0013    0.0064         0         0
    0.0282    0.0161   -0.0287         0
    0.0000    0.0067   -0.0070    0.0130];
                                                                                

Sbar = (eye(4)-P)\P0;
x0   = [Sbar;P(:);Q(:,1);Q(2:4,2);Q(3:4,3);Q(4,4)];

