Another method of gyro integrating.

NigmaTech
Posts: 23
Joined: Wed May 21, 2008 11:53 am

Another method of gyro integrating.

Postby NigmaTech » Wed Aug 13, 2008 4:38 pm

Hi Lubin!

I've created my own method of using gyro and accelerometer to measure tilt. On the begining I was using ADXRS150EB from Analog Devices gyros, but they were failure (manufacturer delivered two times broken units). Now I'm using 2-axial IDG-300 from InvenSense, and MMA7260Q accelerometer from Freescale. Both units has got internal temperature compensation. Unfortunately using VREF to compensate gyro drift is producing greater noises. To measure angles of tilt i've created following model:

Accelero_Gyro_Combo.jpg


AN_11 - X-axis of accelerometer
AN_7 - Z-axis of accelerometer
AN_8 - Y-axis of gyroscope (on the picture is GYRO_X - thats bug)

I'm filtering data using one of yours filters. After start dsPIC is waiting 5 seconds for stabilization our sensors. 5 sec. after start last value logged inside Calibrating block in "Data Store Memory" (ACC_X, GYRO_X), is used inside next block to calibrate data's from Accelerometer and gyroscope. Value inside ACC-Z is loaded manually. Using atan2 function we're computing actual angle from accelerometer. Data from gyro is inntegrated using Accumulation: Forward Euler method. Sum block permits to measure drift in relate to angle from accelerometer. Value inside Gain1 block permits to increse or decrease sensitivity of our gyro_combo, and a bit lets us to control horizontal movement sensitivity.
Unfortunately GIF files with simulation are bigger than 250kB (about 800kB).

What do You think about it Lubin? Have You got any suggestions? Something to correct?

Few examples in JPG's (blue line is a tilt computed using atan2, green is gyro compensated response):

Gain 0.01:
Gain_01.jpg
Gain_01.jpg (61.73 KiB) Viewed 9933 times


Gain 0.003:
Gain_003.jpg
Gain_003.jpg (61.37 KiB) Viewed 9929 times


Gain 0.001:
Gain_001.jpg
Gain_001.jpg (52.33 KiB) Viewed 9926 times


How you see, large values of gain has an effect on large values of phase lead, but on large values of overshoot too.
Any ideas how to damp overshoots?
Attachments
Accelero_Gyro_Combo.mdl
(48.93 KiB) Downloaded 259 times
Last edited by NigmaTech on Thu Aug 14, 2008 10:13 am, edited 1 time in total.

LubinKerhuel
Site Admin - Expert
Posts: 616
Joined: Wed Mar 07, 2007 11:23 pm
Location: Marseille - France
Contact:

Re: Another method of gyro integrating.

Postby LubinKerhuel » Wed Aug 13, 2008 6:48 pm

Hi Nigma,

It's nice to hear from you.
I guess that you manage with the compilation problem you described earlier ;-)

Thanks for sharing this work.

I have only one remark, but as I can't simulate it, you will have to check if I am right :
The low pass filter applied to the gyro may be useless because the integrator already acts as a low pass filter. You may remove it so as to not add phase lag to the system.

The method you describe looks like the one that was described by Pisano (2005) (but I did not manage to get the original paper!)
This filtering method works fines, but when the gyro drift, a static error remains on the angle estimation. I did simulation recently including this method ("Pisano") and I am about to update the IMU page. Feel free to add your comment on this page or on the related discussion page. (Login first on the wiki)
http://www.kerhuel.eu/wiki/Miniature_In ... Unit_-_IMU

I obtain the best results using a structure with a non adaptative kalman structure or with a complementary filter of order 2. (try to publish it online soon!)

Lubin

NigmaTech
Posts: 23
Joined: Wed May 21, 2008 11:53 am

Re: Another method of gyro integrating.

Postby NigmaTech » Thu Aug 14, 2008 12:12 pm

I've updated this post. New pictures. :)

LubinKerhuel
Site Admin - Expert
Posts: 616
Joined: Wed Mar 07, 2007 11:23 pm
Location: Marseille - France
Contact:

Re: Another method of gyro integrating.

Postby LubinKerhuel » Tue Aug 19, 2008 4:15 pm

Hi Nigma,

It's a nice work. Note that .png picture format is the best one for simple graph like the one you uploaded.

I add few remarks hopping that it may help.

- Overshoot is not due to the low pass filter (Filtering here at 30 rad/s ; about 5hz)
- I did not see the calibration of the gyro gain. You may add a gain to obtain the angular rotation speed in a know range. When calibrating the gyro, be careful to precision loss (for example, if you scale gyro with degree/s, use either a fixed point or double variable rather than an int16 data type.
This calibration step may solve the overshoot problem.

- The static difference between the angle estimated from the accelerometers and the reconstructed angle from gyro and accelero should converge toward 0. It seems to remain to a value of about 4-5° here. (Also, calibration of the gyro gain may solve it)

With the calibration done on the accelerometer X axe, the IMU must be at an angle of 0° at initialization time.
In fact, I am not sure that calibration on the accelerometers is necessary.

Not published yet my simulation works, I will try to work at it...

Lubin

LubinKerhuel
Site Admin - Expert
Posts: 616
Joined: Wed Mar 07, 2007 11:23 pm
Location: Marseille - France
Contact:

Re: Another method of gyro integrating.

Postby LubinKerhuel » Wed Aug 20, 2008 8:30 pm

Webpage partly updated. Please have a look and download the simulink file that may help for your projects.

http://www.kerhuel.eu/wiki/Miniature_In ... U#Download

Another look to your structure, I do not understand it. I guess that Gain1 should be the gain of the rate gyro being integrated. Another gain may be added in the feedback loop to control convergence speed. Btw, any gyro bias will produce a static error in the angle estimation (it may be small, depending on the parameters used)

Lubin

NigmaTech
Posts: 23
Joined: Wed May 21, 2008 11:53 am

Re: Another method of gyro integrating.

Postby NigmaTech » Thu Aug 21, 2008 9:22 am

Dear Lubin,

I've got folowing error when trying to open IMU_Simulation.mdl:

error.jpg
error.jpg (30.62 KiB) Viewed 9856 times

LubinKerhuel
Site Admin - Expert
Posts: 616
Joined: Wed Mar 07, 2007 11:23 pm
Location: Marseille - France
Contact:

Re: Another method of gyro integrating.

Postby LubinKerhuel » Thu Aug 21, 2008 12:44 pm

I removed the 'french caracter'. Is it Ok now ?

NigmaTech
Posts: 23
Joined: Wed May 21, 2008 11:53 am

Re: Another method of gyro integrating.

Postby NigmaTech » Wed Sep 24, 2008 9:24 pm

Hi Lubin!

How to use subsystem block "Simplified Kalman for publi" from IMU_Simulation.mdl ??

Bez nazwy.jpg
Bez nazwy.jpg (4.32 KiB) Viewed 9814 times


Where is definied matrix Az, Ts, Kz... ?? How to use it?

LubinKerhuel
Site Admin - Expert
Posts: 616
Joined: Wed Mar 07, 2007 11:23 pm
Location: Marseille - France
Contact:

Re: Another method of gyro integrating.

Postby LubinKerhuel » Wed Sep 24, 2008 10:02 pm

Hi Nigma,

The script Script_Kalman.m define theses constants.

The block "Simplified Kalman for publi" is a simplified version of the block that is really implemented in my IMU.

Despite the IMU described is already working well, it is still under developpment.

Lubin

NigmaTech
Posts: 23
Joined: Wed May 21, 2008 11:53 am

Re: Another method of gyro integrating.

Postby NigmaTech » Wed Sep 24, 2008 10:37 pm

Hi Lubin!

How to compile the file? I have the following error:

error.png

LubinKerhuel
Site Admin - Expert
Posts: 616
Joined: Wed Mar 07, 2007 11:23 pm
Location: Marseille - France
Contact:

Re: Another method of gyro integrating.

Postby LubinKerhuel » Wed Sep 24, 2008 10:54 pm

Hi Nigma !

You have a datatype error. The easiest way to make it works is to do all calculation in double (i.e. open each block ans set data type to double, or use the Simulink Fixed point tool to do all at once...)

If you with to do the calculation in fixed point, avoid to get a fixed point calcul or result data length greater than 32 bits (This is the error shown on your post)

Anyway, the model IMU_dsPIC_30f4012_AlphaBetaFilter_20Mips.mdl implement a floating point version of the filter and the model IMU_dsPIC_30f4012_AlphaBetaFilter_20Mips_FixedPoints.mdl implement a fixed point version, booth for dsPIC 30f4012 (you can compile it, remove few input/output if you are using the dsPIC blockset demo version limited to 6 Input/Output)

Lubin

NigmaTech
Posts: 23
Joined: Wed May 21, 2008 11:53 am

Re: Another method of gyro integrating.

Postby NigmaTech » Wed Sep 24, 2008 11:39 pm

Hi Lubin!

Could you to correct my model? I've compiled it, but dsPIC still displays value -1 on figure (via RS-port), durring random move of IMU. Whats wrong?
Attachments
Accelero_Gyro_Combo_Lubins_Kalman.mdl
(54.85 KiB) Downloaded 186 times

LubinKerhuel
Site Admin - Expert
Posts: 616
Joined: Wed Mar 07, 2007 11:23 pm
Location: Marseille - France
Contact:

Re: Another method of gyro integrating.

Postby LubinKerhuel » Thu Sep 25, 2008 10:05 am

Hi Nigma,

I see at least 1 error in the model:

- The gyro MUST NOT be integrated before the "Non adaptive Kalman block". The gyro in already integrated in this block.

I could give then few advices :
- It is useless to low pass filter the Gyro before the "Non adaptive Kalman block" because this block already does it.
- Same for the Accelerometers. The state space observer takes care of it.

- Why don't you use the UART at a speed of 57600 ? This baud rate is compatible with your 10Mips configuration.
- You should consider using many input of the Tx Output Multiplexed for Matlab/Labview to monitor intermediate variables

Regards
Lubin

NigmaTech
Posts: 23
Joined: Wed May 21, 2008 11:53 am

Re: Another method of gyro integrating.

Postby NigmaTech » Thu Sep 25, 2008 10:55 am

I've removed integrator block before Kalman, Low Pass filters too and nothing changes. dsPIC still displays value "-1" on figure. I'm using 9600 baud rate because when I'm trying to set faster rate, Matlab displays error durring connection:

??? Attempted to access s_control_pos(1); index out of bounds because
numel(s_control_pos)=0.

Error in ==> D:\Program
Files\MATLAB\R2007b\pic\blocks\RS232_analyzBloc.p>RS232_analyzBloc at 75


Error in ==> D:\Program
Files\MATLAB\R2007b\pic\blocks\rs232gui.p>pushStart_Callback at 254


Error in ==> gui_mainfcn at 96
feval(varargin{:});

Error in ==> D:\Program Files\MATLAB\R2007b\pic\blocks\rs232gui.p>rs232gui
at 43


??? Error while evaluating uicontrol Callback

??? Error using ==> serial.fprintf at 143
An error occurred during writing.

Error in ==> D:\Program
Files\MATLAB\R2007b\pic\blocks\rs232gui.p>pushConnexion_Callback at 121


Error in ==> gui_mainfcn at 96
feval(varargin{:});

Error in ==> D:\Program Files\MATLAB\R2007b\pic\blocks\rs232gui.p>rs232gui
at 43


??? Error while evaluating uicontrol Callback

I asked you several times about it, but you didn't answer me.

LubinKerhuel
Site Admin - Expert
Posts: 616
Joined: Wed Mar 07, 2007 11:23 pm
Location: Marseille - France
Contact:

Re: Another method of gyro integrating.

Postby LubinKerhuel » Thu Sep 25, 2008 11:04 am

Hi Nigma,

Matlab use a Java interface to access to the COM port. This interface sometimes bug while trying to connect. In this case, you should press the connect button with the dsPIC off or at least not sending datas. But it seems that you got another error. This may be due to your specific UART PC port.

If you get always the same valueof -1 (which is all 1 in binary), you should try to send directly the ADC result to the UART block to check that UART transmission is working properly. (or try to send a counter, sine wave or whatever. Check the example directory)

Lubin

NigmaTech
Posts: 23
Joined: Wed May 21, 2008 11:53 am

Re: Another method of gyro integrating.

Postby NigmaTech » Thu Sep 25, 2008 12:07 pm

This error appers, just when I'm starting sending data from dsPIC to Matlab (not durring connecting):

??? Attempted to access s_control_pos(1); index out of bounds because
numel(s_control_pos)=0.

Error in ==> D:\Program
Files\MATLAB\R2007b\pic\blocks\RS232_analyzBloc.p>RS232_analyzBloc at 75


Error in ==> D:\Program
Files\MATLAB\R2007b\pic\blocks\rs232gui.p>pushStart_Callback at 254


Error in ==> gui_mainfcn at 96
feval(varargin{:});

Error in ==> D:\Program Files\MATLAB\R2007b\pic\blocks\rs232gui.p>rs232gui
at 43


??? Error while evaluating uicontrol Callback

redefinition SerialCOM

_______________________________________________________________

To communicate dsPIC and PC I'm using BlueSMIRF module, from Sprakfun:

http://www.felix-beck.de/diplom/blog/20 ... bluesmirf/

_______________________________________________________________

Data sended directly from ADC to UART (9600 baud rate):

direct.png


Sine wave:

sine.png
sine.png (6.17 KiB) Viewed 9776 times


Mybe I'm doing something wrong durring compilation? First I'm running Script_Kalman.m file:

run.png
run.png (16.14 KiB) Viewed 9762 times


Matlab displays:

Poles_s =

-0.5000 + 0.8660i
-0.5000 - 0.8660i

Discrete

and, now I'm starting "Incremental build". After writing program to dsPIC I see something that:

cry.png
cry.png (2.7 KiB) Viewed 9759 times


Durring random move of IMU. :cry:

NigmaTech
Posts: 23
Joined: Wed May 21, 2008 11:53 am

Re: Another method of gyro integrating.

Postby NigmaTech » Thu Sep 25, 2008 3:07 pm

OK, Kalman is working, but something's going wrong.
1. IMU can't be longer than 1-2 seconds in near-horizontal position. If it happened figure displays value "-1" and do not move any more. I must to reset dsPIC.

2. There is still drift.

NigmaTech
Posts: 23
Joined: Wed May 21, 2008 11:53 am

Re: Another method of gyro integrating.

Postby NigmaTech » Thu Sep 25, 2008 3:31 pm

The same error appears during using Complementary filter.

LubinKerhuel
Site Admin - Expert
Posts: 616
Joined: Wed Mar 07, 2007 11:23 pm
Location: Marseille - France
Contact:

Re: Another method of gyro integrating.

Postby LubinKerhuel » Thu Sep 25, 2008 3:48 pm

Hi Nigma, is gyro calibration OK ? signal should be in degree/s

NigmaTech
Posts: 23
Joined: Wed May 21, 2008 11:53 am

Re: Another method of gyro integrating.

Postby NigmaTech » Thu Sep 25, 2008 3:48 pm

It looks that:

strange.png


Lines:

Blue - integrated data complementary filtered
Green - accelerometer X-axis
Red - accelerometer Z-axis
and this color between blue and green - gyroscope Y :wink:

NigmaTech
Posts: 23
Joined: Wed May 21, 2008 11:53 am

Re: Another method of gyro integrating.

Postby NigmaTech » Thu Sep 25, 2008 3:56 pm

How do You think, what's the reason of this behavior?

LubinKerhuel
Site Admin - Expert
Posts: 616
Joined: Wed Mar 07, 2007 11:23 pm
Location: Marseille - France
Contact:

Re: Another method of gyro integrating.

Postby LubinKerhuel » Thu Sep 25, 2008 6:34 pm

Hi Nigma,

I am glad to see that you manage to works with the blockset and with the data viewer "rs232gui" interface.
I hope you are enjoying dealing now with the interesting part.

I faced the problem you describe:

If you use only one transfer function to filter and integrate the rate gyro, you may have an internal variable (in the transfer function block) that saturates. This is due to the fact that the transfer function may integrate first the gyro and then apply the high pass filter; thus, the result of the integration will saturate after few seconds or minutes.
The solution is to "cut" the transfer function into two parts: one high pass filter and one integration and to have them in a serial form starting with the high pass filter.
This solution is better but certainly not full proof. Note than even if the integral value is not saturated, it will reach high value that will lower the resolution of the overall calculation (due to floating point calculation)
Check on a long time run your IMU before using it on a flying machine !

The last fixed point state space model in the model IMU_dsPIC_30f4012_AlphaBetaFilter_20Mips_FixedPoints.mdl works well during a long time.

Btw, what component are you dealing with? Are you using a sparkfun module with 2 gyro and 3 accelerometers?

I will be out for 3 days and will not be able to provide more response. I hope this help !

Lubin


Tchangov
Posts: 4
Joined: Thu Feb 12, 2009 6:54 pm

Re: Another method of gyro integrating.

Postby Tchangov » Fri Feb 13, 2009 9:02 pm

Hi Nigma (and Lubin)...

The Integrator (Discrete-Time has a Cte = 0.003 i.e. Time Cte = 1/0.003 = 333s.
Analyze the (SubSystem Feedback) from (In = Output SubSys3) to the (Out= Summing point) from Acc and Gyro:

The G = 0.003 represents the “speed of integration” - very slow;
The Feed back is Unity - Write the Transf Function....Drastically slow dynamic design. (or LOWLOWLOW PASS-never Pass filter).

As Lubin said there is no convergence to Zero Compensation, which is due to slow work of the Integrator.

If you stay long enough time, then the Acc and Gyro sum will be Zeroed by the integrator. Right now the SubSystem including the Integrator have to stay –study state for 5 X 333s = 1666 s = 27 minutes (for 2% accuracy) in order to see it …it is working.

Yes, it works but extremely slow, practically slower than the Gyro Drift.

This is actually the place for Observer-Kalman…you name it.


( Another minor think – the previous DIG Low past filter for the Gyro should be different.)

It is only a hope that this will be (one of the) problem.
Back to Simulation…the rest is easy.
Don't forget to have funnnnn!
Todor

LubinKerhuel
Site Admin - Expert
Posts: 616
Joined: Wed Mar 07, 2007 11:23 pm
Location: Marseille - France
Contact:

Re: Another method of gyro integrating.

Postby LubinKerhuel » Mon May 18, 2009 10:33 pm

Hi Nigma,

Is there any news on the ongoing IMU project ?

NigmaTech
Posts: 23
Joined: Wed May 21, 2008 11:53 am

Re: Another method of gyro integrating.

Postby NigmaTech » Sat Jun 13, 2009 7:46 pm

Hi Lubin!

There's nothing new. I've defended my Master's thesis. IMU project is closed (I hope just temporarily). I'm planning to start new IMU project in a few months and fix one's attention on it only. This time I'm thinking about using 3-Axis Magnetometer from Sparkfun too:

http://www.sparkfun.com/commerce/produc ... cts_id=244

I've bought it last year (with gyro and accelerometer) as a reserve solution, but never used.
I'm still thinking about PhD studies but I'm disgusted after team-work with one guy on M.A. Now I must to cool down, maybe next year.

Regards Michael :D

melodyincognito
Posts: 1
Joined: Sun Jun 21, 2009 8:55 am

Re: Another method of gyro integrating.

Postby melodyincognito » Sun Jun 21, 2009 9:03 am

Hi all!
Does anyone know how to conpensate gyro drift using particle filter...like there are great resources on the net posted by lubin and many others using 6DOF imu... I want a simulink model doing the same task using particle filter... If anyone has worked on it earlier...kindly share ur work...!
Thanks!


Return to “Advanced Simulink & RTW Questions”

Who is online

Users browsing this forum: No registered users and 1 guest