Accelerometer + Gyro Kalman Filter

[Kilometers]

Hi Luka,

You can find some places to get ideas on how to implement Kalman Filters well in LabVIEW in earlier posts in the thread from Vivek_N and DjDaveNI. In terms of implementing it in LabVIEW RT it should be very similar. LabVIEW Real-Time can do most of what normal LabVIEW can. As for unsupported features, that depends on what OS your device is running. You can find unsupported features by OS in these two links, http://zone.ni.com/reference/en-XX/help/370622J-01/lvrtconcepts/rt_unsupportedets/ and http://zone.ni.com/reference/en-XX/help/370622J-01/lvrtconcepts/rt_unsupportedvxworks/. 

In order to tell the best way to set up your code it would be best to know a bit more about your application. Why do you need to use Real-Time? When designing in Real-Time it is usually best to narrow down your single most import process (for example a safety shutoff, or a PID controller). Are you using this filtered data for anything immediately that you need to have it filtered deterministically rather than just in a normal while loop? Let us know a bit more about your application and why you moved to a Real-Time system, and we may be able to help you with suggestions on code set up.  

[LukaAmbrozic]

Miles, HI!

The Kalman filter is, for now, just a converter from raw data (gyro, acc and mag) to estimated data such as orientation or angles of the IMU units. We need this to run in real time, because these estimated values will be part of the input to the control system, and they should be up to date with the current events in the real world. We need it running at 100 Hz. The point is, that we have a custom Kalman, which is not, as far as I know, implemented in the module people are referring to.

This is the process:

We have everything laready running on an xPC Target RTOS from MAthWorks - basically it was implemented in simulink. We have the Kalman filter m code and we're trying to replicate the system to a cRIO device, since the end system (Controller, etc), which is larger, will be implemented using RIO RT, and we need to be able to port our code to RIO for integration with those, who are working on the control system.

Our part is acquisition of data through analog inputs and through UDP (for now, later through SPI). The demands of the system are, that the system should be able to run 9 separate Kalman filter for 9 separate IMUs in parallel, or in serial, as long as the estimated data is available at any given sample of 100Hz.

Now, we are using  a MathScript Node for the Kalman filter., which I believe are not really suitable for RT, but should work?!

When testing it, with constant data inputs, the Kalman filter operated at a high frequency in a timed loop on cRIO. When trying to run 9 clones of the Kalman, the timed loop dis not operate in the time necessary (it was more like 1Hz instead of 100Hz). The filter VI is set as reentrant clone.

Also, the first 100 samples should only be used for the initialization of Kalman parameters, such as bias and gains, and all the rest should pass through Kalman. This is, for the moment, implemented with a case structure based on tick count of the main loop.

Our cRIO is running VxWorks RTOS, I believe.

Your thoughts on the subject are highly appreciated!

Thanks,

Luka

[User002]

---

@LukaAmbrozic wrote:

Miles, HI!

The Kalman filter is, for now, just a converter from raw data (gyro, acc and mag) to estimated data such as orientation or angles of the IMU units. We need this to run in real time, because these estimated values will be part of the input to the control system, and they should be up to date with the current events in the real world. We need it running at 100 Hz. The point is, that we have a custom Kalman, which is not, as far as I know, implemented in the module people are referring to.

This is the process:

We have everything laready running on an xPC Target RTOS from MAthWorks - basically it was implemented in simulink. We have the Kalman filter m code and we're trying to replicate the system to a cRIO device, since the end system (Controller, etc), which is larger, will be implemented using RIO RT, and we need to be able to port our code to RIO for integration with those, who are working on the control system.

Our part is acquisition of data through analog inputs and through UDP (for now, later through SPI). The demands of the system are, that the system should be able to run 9 separate Kalman filter for 9 separate IMUs in parallel, or in serial, as long as the estimated data is available at any given sample of 100Hz.

Now, we are using  a MathScript Node for the Kalman filter., which I believe are not really suitable for RT, but should work?!

When testing it, with constant data inputs, the Kalman filter operated at a high frequency in a timed loop on cRIO. When trying to run 9 clones of the Kalman, the timed loop dis not operate in the time necessary (it was more like 1Hz instead of 100Hz). The filter VI is set as reentrant clone.

Also, the first 100 samples should only be used for the initialization of Kalman parameters, such as bias and gains, and all the rest should pass through Kalman. This is, for the moment, implemented with a case structure based on tick count of the main loop.

Our cRIO is running VxWorks RTOS, I believe.

Your thoughts on the subject are highly appreciated!

Thanks,

Luka

---

Perhaps you are experiencing this problem?

http://digital.ni.com/public.nsf/allkb/C0EA4D80188C49D6862578650012406D

Br,

/Roger

[LukaAmbrozic]

Roger, HI!

I read all about your port of the similar filter (Which we have tested as well, and is great) to LV. We developed our own IMUs and the filter works great. Only ours works better also in static conditons (the error on that one while being still, is over 10deg, ours is about 0,5 deg).

As to your question about the MS Node - I have all the code inline and do not use any of the described functions, that would cause a slow down. Also, the node does not show any warning glyphs.

I would say, my problem is more with the overall concept of implementation logic in LV and Real Time. Since you have a lot of experience in this matter, perhaps you could offer some insight into RT or FPGA programming of the Kalman filter on cRIO? It's not crucial yet, but I would like to have it running somewhere in the near future.

The logic of the filter is:

First 100 samples determine different means, bias, gains, state init... then they're passed to the UKF, which calculates the estimated quaternion for the ongoning samples. I'm pretty sure I made some mistake with the concept/design of the VI, but I ran the filter with constant values for the gyro, acc and mag on the cRIO and it seemed to work OK. Running the same deal, only with 9 filters in parallel - (like having 9 IMUs at once) - slows down extremely.

That is all I can tell you for now. I've been on this for about a month, and am self-taught.

Regards,

Luka

[User002]

---

@LukaAmbrozic wrote:

Roger, HI!

I read all about your port of the similar filter (Which we have tested as well, and is great) to LV. We developed our own IMUs and the filter works great. Only ours works better also in static conditons (the error on that one while being still, is over 10deg, ours is about 0,5 deg).

As to your question about the MS Node - I have all the code inline and do not use any of the described functions, that would cause a slow down. Also, the node does not show any warning glyphs.

I would say, my problem is more with the overall concept of implementation logic in LV and Real Time. Since you have a lot of experience in this matter, perhaps you could offer some insight into RT or FPGA programming of the Kalman filter on cRIO? It's not crucial yet, but I would like to have it running somewhere in the near future.

The logic of the filter is:

First 100 samples determine different means, bias, gains, state init... then they're passed to the UKF, which calculates the estimated quaternion for the ongoning samples. I'm pretty sure I made some mistake with the concept/design of the VI, but I ran the filter with constant values for the gyro, acc and mag on the cRIO and it seemed to work OK. Running the same deal, only with 9 filters in parallel - (like having 9 IMUs at once) - slows down extremely.

That is all I can tell you for now. I've been on this for about a month, and am self-taught.

Regards,

Luka

---

Hi Luka, cool that you have tried out the ahrs stuff I put on the community. 

It is far from perfect. What is missing is a good calibration routines. Now it's more or less very much a hands-on excercise to get any presicion at all from the algorithm. Find a good way of calibrating the stuff and you will get a decent performance from it. I think Sebastian Madgwick have outlined this in his research paper:

http://www.x-io.co.uk/res/doc/madgwick_internal_report.pdf (static presicion in page 21)

Basically what I have done is to manually, and very unscientific, position the IMU so that I can find the minima & maxima of the sensors, and then hand-tune it so that I can get decent static presicion from it.

Back to your problem, I would perhaps port the kalman filter to labview. It shouldn't be too much of a task in your case I suppose?

I would start by removing some functionality from the Math Script code and implement it in LabVIEW, then make it work toghether. Then remove some more and implement in LV, and iterate.

Br,

/Roger

[LukaAmbrozic]

Hi, roger,

We have advanced methods to calibrate the MEMS sensord on the IMU, most notably calibration for the magnetometer. The errors are reall minimal in dynamic situation, best from every option, we tested. The problem is, that in static conditions, there seems to be a lot of noise. I believe, our version was the Mahoney algorithm...

As to the UKF problem... I had the same thought about porting the functionality of the m code to labView blocks, but it seems to be a time-consuming task, and I have limited time, so i thought I'd use MS nodes as a shortcut. NI promises they work... they should, shouldn't they?

Any suggestions how to implement the first 100 samples initialization nad then pass the values on to the UKF? In RT and with a timing of 100Hz.

Thanks,

Luka

[User002]

---

@LukaAmbrozic wrote:

Hi, roger,

We have advanced methods to calibrate the MEMS sensord on the IMU, most notably calibration for the magnetometer. The errors are reall minimal in dynamic situation, best from every option, we tested. The problem is, that in static conditions, there seems to be a lot of noise. I believe, our version was the Mahoney algorithm...

As to the UKF problem... I had the same thought about porting the functionality of the m code to labView blocks, but it seems to be a time-consuming task, and I have limited time, so i thought I'd use MS nodes as a shortcut. NI promises they work... they should, shouldn't they?

Any suggestions how to implement the first 100 samples initialization nad then pass the values on to the UKF? In RT and with a timing of 100Hz.

Thanks,

Luka

---

I think the noise could be originating from any discrepancy between the positioning of the magnetometer and accelerometer XYZ axis. The algorithm will not converge to the minima and instead give the apparance of noise as it tries to converge. This is what led me to hand-tune so that the axis are aligned perfectly, with very little algorithm noise. Perhaps this is what you already do in the calibration technology you have?

We can discuss about what NI promises and what they deliver, but that is another topic. Perhaps they missed out on testing out the performance, as per usual, on the math script nodes?

I know that matlab historically have been very fast on array operations, but extremely slow on branching (if statements) and loops, see if you can get rid of those and make all code do array operations only, if possible.

Do you want to initialize the UKF filters with predefined 100 samples and then run it at 100Hz with measurement data from the sensors? Did I understand it correctly?

Br,

/Roger

[LukaAmbrozic]

Roger,

NO, the first samples come one by one from the sensors. They are not predefined. After 100 first good samples, the Kalman enables and the feedback nodes (1/z delays) get initialized to the values calculated from the first 100 samples.

In LV, it seems that this is hard to do, since the feedback nodes get initialized only once if on the outside; if inside a case structure, they do so every iteration 🙂 But that's just one problem. I could even run a trace on the cRIO kalman, just nothing got back...

Maybe you should take a look at my code (lousy, crappy code) 😄 posted in one of my previous posts, that will give you the idea I'm following.

Talk to you 2morrow, probably.

BR,

Luka

[User002]

---

@LukaAmbrozic wrote:

Roger,

NO, the first samples come one by one from the sensors. They are not predefined. After 100 first good samples, the Kalman enables and the feedback nodes (1/z delays) get initialized to the values calculated from the first 100 samples.

In LV, it seems that this is hard to do, since the feedback nodes get initialized only once if on the outside; if inside a case structure, they do so every iteration 🙂 But that's just one problem. I could even run a trace on the cRIO kalman, just nothing got back...

Maybe you should take a look at my code (lousy, crappy code) 😄 posted in one of my previous posts, that will give you the idea I'm following.

Talk to you 2morrow, probably.

BR,

Luka

---

Hi Luka, I fixed the issue you had with the feedback nodes. I replaced them with shift registers in a while loop. They can be initialized at any time from what you have inside the math script node(s).

Check if you can follow my code on how to use shift registers in while loops that iterates only once. They are a bit more powerful programattically than the feedback nodes.

I also moved all code to the host to be able to test running on my windows machine. And in your case its a good starting point before trying it on the cRIO. Also, the IO's can be read over the network on the windows machine.

If you notice any CPU load above 10-20% on a fairly modern machine (core i5/i7 with lots of ram), then you are bound to have problems when running on that NI-cRIO9025 with an 800Mhz controller.

The main problem with the performance that you are noticing is indeed in the Math Script nodes. And if you want to run the Kalman filters in 100Hz, you probably need to hone your LabVIEW skills and re-implement those using G-wiring.

See the attachment for the modified code.

Br,

/Roger

[User002]

Another thing that just hit me.

Have you tried to put all the kalman filters into one Math Script node, maybe this could reduce the CPU usage?

Instead of 9 calls to the Math Script node, one would be enough.

Not sure where the bottleneck is, though.

If it is the calls to the Math Script node itself, then it could work.

Alternatively the code inside the Math Script node, then probably not.

Try to remove all looping and conditionals (for and if statements) inside the script nodes.

Last time I worked with Matlab everything beside array operations were slow.

Could prove to be the same for the code you have in the Math Script node.

Br,

/Roger