Free integration calculation issue

[Chris2L]

I stumble upon an issue in free integration as my program calculated the same problem.

I have the following motion file

ini lat (deg),ini lon (deg),ini alt (m),ini vx_body (m/s),ini vy_body (m/s),ini vz_body (m/s),ini yaw (deg),ini pitch (deg),ini roll (deg)
0,0,0,100,0,0,0,0,0
command type,yaw (deg),pitch (deg),roll (deg),vx_body (m/s),vy_body (m/s),vz_body (m/s),command duration (s),GPS visibility
1,18,0,0,0,0,0,10,0
1,0,0,0,0,0,0,10,0

Basically running North at 100 m/s at init, making a 180 deg turn to go South. After the turn I have a residual 0.314 m/s velocity in the East direction. If I then continue for 10s, my position is off by about 4 m. See the max error in the following output:

The following are error statistics.
-----------statistics for simulation attitude (Euler, ZYX)  from algo (in units of ['deg', 'deg', 'deg'])
        Simulation run algo0_0:
                --Max error: [6.63957329e-09 9.04183229e-06 4.23312952e-05]
                --Avg error: [-1.22998502e-09 -4.51875153e-06  1.69835339e-05]
                --Std of error: [1.92722543e-09 4.93997734e-06 1.31422442e-05]

-----------statistics for simulation position from algo (in units of ['m', 'm', 'm'])
        Simulation run algo0_0:
                --Max error: [1.00223908 4.70682951 0.00532531]
                --Avg error: [ 7.50355098e-01  1.80151065e+00 -1.42031785e-03]
                --Std of error: [0.35447558 1.52014583 0.0015642 ]

-----------statistics for simulation velocity from algo (in units of ['m/s', 'm/s', 'm/s'])
        Simulation run algo0_0:
                --Max error: [0.15715554 0.31415198 0.00068664]
                --Avg error: [ 0.050112    0.23549853 -0.0002667 ]
                --Std of error: [0.06053796 0.11113709 0.00021853]

The position error will obviously grow over time. I run the code as follows for the above result:

                vel_dot_n = c_bn.T.dot(accel[i-1, :]) + g_n -\
                            attitude.cross3(2*w_ie_n + w_en_n, self.vel[i-1, :])
                # Current velosity is previous plus my experienced changed in velocity
                self.vel[i, :] = self.vel[i-1, :] + vel_dot_n * self.dt

                #### propagate position
                lat_dot = self.vel[i-1, 0] / rm_effective
                lon_dot = self.vel[i-1, 1] / rn_effective / cl
                alt_dot = -self.vel[i-1, 2]
                self.pos[i, 0] = self.pos[i-1, 0] + lat_dot * self.dt
                self.pos[i, 1] = self.pos[i-1, 1] + lon_dot * self.dt
                self.pos[i, 2] = self.pos[i-1, 2] + alt_dot * self.dt
                #### output
                c_bn = attitude.euler2dcm(self.att[i, :])

In the free integration file, there was some code commented out that updates the c_bn first, calculates vel_dot_b and then vel_b, then converts the vel_b to vel, and updates the position. If I do the calculation this way around it works out correctly. See the output below:

The following are error statistics.
-----------statistics for simulation attitude (Euler, ZYX)  from algo (in units of ['deg', 'deg', 'deg'])
        Simulation run algo0_0:
                --Max error: [0.00000000e+00 1.32311075e-12 1.01268861e-11]
                --Avg error: [ 0.00000000e+00 -5.00873751e-13 -3.77686476e-12]
                --Std of error: [0.00000000e+00 3.71630599e-13 3.40864583e-12]

-----------statistics for simulation position from algo (in units of ['m', 'm', 'm'])
        Simulation run algo0_0:
                --Max error: [8.41556947e-08 1.12544081e-06 4.64393640e-04]
                --Avg error: [ 2.21354525e-08 -4.37359926e-07 -3.47980894e-04]
                --Std of error: [2.92403334e-08 3.63927099e-07 1.64325317e-04]

-----------statistics for simulation velocity from algo (in units of ['m/s', 'm/s', 'm/s'])
        Simulation run algo0_0:
                --Max error: [1.77236092e-09 7.36129863e-08 7.29224333e-05]
                --Avg error: [-3.67040030e-10 -5.51504148e-08 -2.32196570e-05]
                --Std of error: [6.70454848e-10 2.61383274e-08 2.81123335e-05]

I change the code to the following for this result:

                c_bn = attitude.euler2dcm(self.att[i, :])
                vel_dot_b = accel[i-1, :] + c_bn.T.dot(g_n) -\
                            attitude.cross3(c_bn.dot(w_ie_n)+gyro[i-1,:], self.vel_b[i-1,:])
                self.vel_b[i,:] = self.vel_b[i-1,:] + vel_dot_b*self.dt
                self.vel[i,:] = c_bn.T.dot(self.vel_b[i, :])
                
                # vel_dot_n = c_bn.T.dot(accel[i-1, :]) + g_n -\
                #             attitude.cross3(2*w_ie_n + w_en_n, self.vel[i-1, :])

                # # Current velosity is previous plus my experienced changed in velocity
                # self.vel[i, :] = self.vel[i-1, :] + vel_dot_n * self.dt

                #### propagate position
                lat_dot = self.vel[i-1, 0] / rm_effective
                lon_dot = self.vel[i-1, 1] / rn_effective / cl
                alt_dot = -self.vel[i-1, 2]
                self.pos[i, 0] = self.pos[i-1, 0] + lat_dot * self.dt
                self.pos[i, 1] = self.pos[i-1, 1] + lon_dot * self.dt
                self.pos[i, 2] = self.pos[i-1, 2] + alt_dot * self.dt
                #### output
                # c_bn = attitude.euler2dcm(self.att[i, :])

I made the IMU and GPS noise 0 to remove inconsitancy in the calculation and I am running the sim in
ref_frame=0,
in the demo_free_integraion file.

Is this problem an issue in the simulator generating the data or in the way free integration calculates?

[Chris2L]

Changing the corrected code to the following:

                vel_dot_b = accel[i-1, :] + c_bn.T.dot(g_n) -\
                            attitude.cross3(c_bn.dot(w_ie_n)+gyro[i-1,:], self.vel_b[i-1,:])
                self.vel_b[i,:] = self.vel_b[i-1,:] + vel_dot_b*self.dt
                c_bn = attitude.euler2dcm(self.att[i, :])
                self.vel[i,:] = c_bn.T.dot(self.vel_b[i, :])

(moving c_bn update down) gives a near-perfect result for free_integration:

The following are error statistics.
-----------statistics for simulation attitude (Euler, ZYX)  from algo (in units of ['deg', 'deg', 'deg'])
        Simulation run algo0_0:
                --Max error: [0. 0. 0.]
                --Avg error: [0. 0. 0.]
                --Std of error: [0. 0. 0.]

-----------statistics for simulation position from algo (in units of ['m', 'm', 'm'])
        Simulation run algo0_0:
                --Max error: [0. 0. 0.]
                --Avg error: [0. 0. 0.]
                --Std of error: [0. 0. 0.]

-----------statistics for simulation velocity from algo (in units of ['m/s', 'm/s', 'm/s'])
        Simulation run algo0_0:
                --Max error: [0.00000000e+00 5.91110382e-20 2.13906583e-16]
                --Avg error: [ 0.00000000e+00  9.42872039e-21 -4.45712803e-17]
                --Std of error: [0.00000000e+00 1.41265006e-20 9.44778169e-17]

This is more in line with how it is done in the ref_frame=1, section of the code of free integration.

EDIT:
For my example there was an error in the code that did not affect the result as everything happened in the horizontal plane, but c_bn.T.dot(g_n) the .T should be removed.

[dxg-aceinna]

This is only a simulation. To recover 100% the positions in free integration with the simulated data, we need to make sure the data is integrated the same way as it is derived from pos/vel/att. Otherwise, numerical integration and numerical accuracy can cause difference. You can verify this by setting a higher sampling rate to see if this helps.

[Chris2L]

Ok, great, seems like it is an integration error. There is another issue I updated in my code to change pos update to:
pos(k) = pos(k-1) + vel(k-1)*dt + 0.5*acc*dt*dt
I understand that this is more in line with kinematics.