Why isn't there any integration drift in simulation of Inertial Navigation System(INS)?

In INS, we obtain measurements from accelerometer and gyroscope; by integrating these the position and orientation of an UAV can be determined. I have simulated the dynamical equations for a point mass model of UAV. To simulate the noise, I added noise to the dynamical equations of velocity and angle rates. Since I want the UAV to track a particular trajectory, there are control laws in loop. When I integrate these equations, I only see noise on top of all the states, but not divergence in any of them. Why isnt the integrated error diverging?

I tried to add Kalman filter to this implementation, by assuming we can measure velocity, heading angle and the altitude of the UAV. Even this formulation is giving similar results with only extra noise in the integrated states.

My doubt is, since there are controllers in loop, at every time step, they are trying to ensure that the desired path is tracked and hence they are ensuring the UAV does not drift from the original path. This argument supports my simulation results. However, I am not sure about this logic. Can someone please explain?

[Brian Fanous]

Shubham

Can you clarify what feature you are using and in which toolbox? Are you talking about the INS block?

[Shubham Kalpande]

I would be really grateful to you if you could help me out.

I am trying to simulate the dynamics of an UAV represented by the following equations.

I want to see if there is no position feedback, and the UAV makes use of only IMU measurements, how does the error propagate. To do this, I added white noise to the equations of v_dot, alpha_dot and mu_dot. Assuming I can measure velocity, altitude and heading, I created a Kalman filter and simulated the equations. Now, as there is measurement noise and process noise, with position measurements, the position estimates obtained from the Kalman Filter should drift with time. However, I am not observing this expected behaviour.

I used the UAV toolbox to make the UAV follow a set of waypoints. I am attaching the code as well.

apna_kalman_cf.m - this is the file where i am integrating the dynamical equations and kalman filter is implemented

fx_apna_kalman_cf.m - this is the function file, where all the equations are defined along with the control laws

Kindly suggest me what is wrong with my implementation and why I am not getting any significant drift/divergence in the position estimates.

I have referred to this example from the UAV toolbox, and trying to develop something from it.

To run my simulation, have all the files at 1 location and run the following code:

ss = ExampleHelperUAVStateSpace("MaxRollAngle",pi/6,...
"AirSpeed",4.5,...
"FlightPathAngleLimit",[-0.2 0.2],...
"Bounds",[-50 10000; -50 10000; -50 100; -pi pi]);
waypoints=[0 0 10 0; 15000 0 10 0;];
apna_kalman_cf

Answer 1

[Jianxin Sun]

Hi Shubham,

A typical control loop setup would look like:

Plant should be your ODE equations with no addtitive noise.

Then you can add noise to the ground truth output from the plant in the sensor model and use filter to create estimated states. To see the drift you expected, you need to implement the IMU model in the Sensor block. You can use the imuSensor object from Navigation toolbox to achieve this. imuSensor takes in ground truth acceleration and angular velocity and add noise to them.

Then in the filter block, you will compute the estimated x-y-z position based on the measured acceleration coming from imuSensor and this is where you should see drift between your estimated x-y-z position and true x-y-z position.

These estimated states then will be used to generate your control signal T_c, alpha_c and mu_c.

Thanks,

Jianxin