Kalman Filter for XY Streams

[Rick19468]

Hi!

I want to input two location data output streams (X,Y from GPS) and (X,Y from a nine axis composite data gyro+accelerometer+compass) into this Kalman filter.
The objective here is to have the motion tracking device supplement the GPS.

My question is , How do I attach these X,Y streams onto this filter?

Thanks!

#include <cstdlib>
#include <stdio.h>
#include <stdlib.h>
#include <math.h>


 


double frand() {


return 2*((rand()/(double)RAND_MAX) - 0.5);


}


 


int main() {
using namespace std;




//initial values for the kalman filter


float x_est_last = 0;


float P_last = 0;


//the noise in the system


float Q = 0.022;


float R = 0.617;


 


float K;


float P;


float P_temp;


float x_temp_est;


float x_est;


float z_measured; //the 'noisy' value we measured


float z_real = 0.5; //the ideal value we wish to measure


 


srand(0);


 


//initialize with a measurement


x_est_last = z_real + frand()*0.09;


 


float sum_error_kalman = 0;


float sum_error_measure = 0;


 


for (int i=0;i<30;i++) {


//do a prediction


x_temp_est = x_est_last;


P_temp = P_last + Q;


//calculate the Kalman gain


K = P_temp * (1.0/(P_temp + R));


//measure


z_measured = z_real + frand()*0.09; //the real measurement plus noise


//correct


x_est = x_temp_est + K * (z_measured - x_temp_est);


P = (1- K) * P_temp;


//we have our new system


 


printf("Ideal    position: %6.3f \n",z_real);


printf("Mesaured position: %6.3f [diff:%.3f]\n",z_measured,fabs(z_real-z_measured));


printf("Kalman   position: %6.3f [diff:%.3f]\n",x_est,fabs(z_real - x_est));


 


sum_error_kalman += fabs(z_real - x_est);


sum_error_measure += fabs(z_real-z_measured);


 


//update our last's


P_last = P;


x_est_last = x_est;


}


 


printf("Total error if using raw measured:  %f\n",sum_error_measure);


printf("Total error if using kalman filter: %f\n",sum_error_kalman);


printf("Reduction in error: %d%% \n",100-(int)((sum_error_kalman/sum_error_measure)*100));


 




return 0;


}

[Salem]

Perhaps you could enlighten us by saying whether you want C or C++ code.

In the meantime, your code - indented

#include <cstdlib>
#include <stdio.h>
#include <stdlib.h>
#include <math.h>

double frand()
{
  return 2 * ((rand() / (double) RAND_MAX) - 0.5);
}

int main()
{
  using namespace std;

  //initial values for the kalman filter
  float x_est_last = 0;
  float P_last = 0;
  //the noise in the system
  float Q = 0.022;
  float R = 0.617;
  float K;
  float P;
  float P_temp;
  float x_temp_est;
  float x_est;
  float z_measured;             //the 'noisy' value we measured
  float z_real = 0.5;           //the ideal value we wish to measure

  srand(0);

  //initialize with a measurement
  x_est_last = z_real + frand() * 0.09;

  float sum_error_kalman = 0;
  float sum_error_measure = 0;

  for (int i = 0; i < 30; i++) {
    //do a prediction
    x_temp_est = x_est_last;
    P_temp = P_last + Q;

    //calculate the Kalman gain
    K = P_temp * (1.0 / (P_temp + R));

    //measure
    z_measured = z_real + frand() * 0.09; //the real measurement plus noise

    //correct
    x_est = x_temp_est + K * (z_measured - x_temp_est);
    P = (1 - K) * P_temp;

    //we have our new system
    printf("Ideal    position: %6.3f \n", z_real);
    printf("Mesaured position: %6.3f [diff:%.3f]\n", z_measured, fabs(z_real - z_measured));
    printf("Kalman   position: %6.3f [diff:%.3f]\n", x_est, fabs(z_real - x_est));

    sum_error_kalman += fabs(z_real - x_est);
    sum_error_measure += fabs(z_real - z_measured);

    //update our last's
    P_last = P;
    x_est_last = x_est;
  }

  printf("Total error if using raw measured:  %f\n", sum_error_measure);
  printf("Total error if using kalman filter: %f\n", sum_error_kalman);
  printf("Reduction in error: %d%% \n", 100 - (int) ((sum_error_kalman / sum_error_measure) * 100));

  return 0;
}

[Nominal Animal]

Unfortunately, that will not work for that; it seems to be an implementation for just one static variable.

First of all, you need much more internal state to be able to use all the sensors: location (x, y, z), velocity (ẋ, ẏ, ż), acceleration (ẍ, ÿ, z̈), orientation (α, φ, ω), angular velocity (α̇, φ̇, ω̇), and angular acceleration (α̈, φ̈, ω̈).

Exactly how your gyro provides the orientation, GPS the location, and accelerometer the acceleration and angular acceleration (and whether it provides the velocity and/or angular velocity too), determines which coordinate systems and units you need to use for that state.

What you need is a Kalman filter which uses location, acceleration, and angular acceleration as inputs, and provides at minimum the (filtered) location (but due to physics, it will also have to estimate the velocity, angular velocity, and orientation). The fact that you only need two of the location coordinates, does not make it any simpler; you need the full state I described to get maximum use out of your sensors.

You will first have to find out the exact form of the Kalman filter that applies to this situation. I think math or physics forums would be more suitable for this than this one. (The filter uses Newtonian equations of motion to estimate the new state, with the estimate and measurements combined using simple math to provide the result. I think it's more physics than math.) You might also wish to check the documentation of the sensors/chips; they usually have at least useful pointers to discussions and explorations on the theoretical side, and often examples, too.

When you have the equations you wish to implement -- but remember that they depend on the details of your sensors, and especially the units/coordinate systems they use! --, then we can surely help you with the programming implementation, but I don't think this forum is optimal for trying to find out the exact mathematical formulae for this Kalman filter. One step at a time!

[Rick19468]

@Nominal Animal: "What you need is a Kalman filter which uses location, acceleration, and angular acceleration as inputs, and provides at minimum the (filtered) location (but due to physics, it will also have to estimate the velocity, angular velocity, and orientation). The fact that you only need two of the location coordinates, does not make it any simpler; you need the full state I described to get maximum use out of your sensors."

I believe what your stating here is already done with the on chip "MotionFusion algorithms" of the MPU-9150. All the devices on the chip (gyro,accelerometer and digital compass) are already interfaced, optimized and kalman filtered, etc.


INVENSENSE:
"The MPU-9150 is a System in Package (SiP) that combines two chips: the MPU-6050, which contains a 3-axis gyroscope, 3-axis accelerometer, and an onboard Digital Motion Processor™ (DMP™) capable of processing complex 9-axis MotionFusion algorithms; and the AK8975, a 3-axis digital compass. The part’s integrated 9-axis MotionFusion algorithms access all internal sensors to gather a full set of sensor data. The part is offered in a 4x4x1mm LGA package and is upgrade-compatible with the MPU-6050™ integrated 6-axis MotionTracking device, providing a simple upgrade path and making it easy to fit on space constrained boards.
The InvenSense MotionApps™ Platform that comes with the MPU-9150 abstracts motion-based complexities, offloads sensor management from the operating system and provides a structured set of APIs for application development.
For precision tracking of both fast and slow motions, the parts feature a user-programmable gyro full-scale range of ±250, ±500, ±1000, and ±2000°/sec (dps), a user-programmable accelerometer full-scale range of ±2g, ±4g, ±8g, and ±16g, and compass with a full scale range of ±1200µT."


Of my beginners level rational,

My thoughts are this, I have two "stand alone" fully interfaced and optimized data sources for x,y location output streams. There is no further optimization to be done........ individually. Both units (GPS module with its Precision Point Positioning algorithms and the MPU-9150 with its Motion Fusion algorithms) the outputs are at their best your gone to get from these modules.

My question is this,
(My very possibly wrong understanding is that the MPU-9150 provides the more accurate location in the short term (<10 secs) .... after getting its "fix" of course)

With a simple two input Kalman filter, can I not now merge the best attributes of both devices?



If yes, I like to explore a simple two input c code Kalman.
If not or so mathematically complex .... it exceeds the scope of this forum.

[Rick19468]

The VN-200 incorporates the latest advancements in Aerospace grade Kalman filter inertial navigation algorithms specifically tailored to solve problems typically faced by industrial users.Industrial control applications require high accuracy low latency position, velocity, and attitude measurements. GPS provides long-term position accuracy, but is incapable of providing low-latency high bandwidth measurements. Inertial sensors provide high bandwidth acceleration and angular rates, but cannot provide long-term position and orientation accuracy due to drift and integration errors. Utilizing an onboard Extended Kalman Filter the VN-200 is capable of combining in an optimal manner the high bandwidth inertial measurements with the high accuracy low bandwidth GPS measurements. The VN-200 provides industrial users with access to a continuous stream of low latency high accuracy position velocity and attitude measurements at a rate of up to 200Hz.

I'm trying to do this with these two (GPS + IMU) modules ...... but for cheap! : )

VN-200 Rugged - VectorNav Technologies

[Nominal Animal]

I believe what your stating here is already done with the on chip "MotionFusion algorithms" of the MPU-9150.

For the sensors, yes; it certainly looks like that, good. I checked out the PDFs I could without creating an account, but they don't seem to list exactly which data and which format you can get out of it. However, for the purposes of this thread, I'll assume it gives you at least the X, Y, and Z coordinates.

If the coordinate system is not aligned so that Z is vertical, you'll need to rotate it, but that is a simple mathematical operation. You won't need trigonometric functions, and I'd be happy to show how to do that if it becomes relevant. Knowledge of basic vector calculus (the concept of vectors, dot product, and cross product) is very useful in that case.

However, the MPU-9150 does not seem to be able to include the GPS data for its Kalman filter.

My thoughts are this, I have two "stand alone" fully interfaced and optimized data sources for x,y location output streams. There is no further optimization to be done........ individually.

I agree. However, the current situation is more complicated, in that only the GPS data has noise, whereas the MPU-9150 has only drift.

With a simple two input Kalman filter, can I not now merge the best attributes of both devices?

Yes and no. No, because a Kalman filter is basically a tool developed to reduce noise, and it assumes the noise is normally distributed in all inputs. Yes, because I think you could model the drift based on a Kalman filter, using the GPS coordinates as input.
_ _ _ _ _

Here is how I think it would work. If you agree and understand, then we can look at how to write it in code. (English is not my native language, so if you need me to rephrase any of this, just let me know and I'll try.)

If MPU-9150 provides you with absolute coordinates, say mpu_x, mpu_y, mpu_z, we can use those directly. (If MPU-9150 provides you with absolute movement since last report, say delta_x, delta_y, delta_z, then we just do mpu_x+=delta_x, mpu_y+=delta_y and mpu_z+=delta_z to get those coordinates, starting at zeros when the system boots.)

The problem now is to use the GPS coordinates to make sure the origin (0,0,0) for mpu_x, mpu_y, and mpu_z stays in the same physical point, and does not drift. Lets name the adjustment origin_x, origin_y and origin_z; they are in MPU-9150 units. (Let's name the GPS coordinates for the origin gps0_x, gps0_y, gps0_z.)

If the adjustment is correct for this moment, then we are at coordinates mpu_x-origin_x, mpu_y-origin_y, mpu_y-origin_y. As time progresses, the adjustment origin_y, origin_y and origin_z will change only slowly, at the rate we think the MPU-9150 coordinates may drift.

If we transform the coordinates back to GPS units, add the location of origin in GPS units, and subtract the current GPS coordinates, we get the current GPS positioning noise. It should be pretty random, centered around origin, plus-minus the expected GPS precision.

A trivial filter would simply add say 1% of that noise back to the adjustment coordinates in MPU-9150 units. This works best if the GPS reports coordinates that fluctuate. If the coordinates are rounded, then that will adversely affect the drift adjustment -- but I don't see how a Kalman filter would help there either. (In a way, you might say a Kalman filter adjusts the ratio dynamically. If we can estimate the drift rate based on the MPU-9150 velocity or acceleration data, a Kalman filter might give even better results.)

If the GPS does resolve to say 1" precision, but fluctuates, then the above filtering should work very well. If the GPS reports coordinates rounded to say 1' precision, then the above filtering will mean that when the dog sleeps, the coordinates will slowly adjust so that the dog position is in the center of the GPS coordinates.

[Rick19468]

Question:

I'm now thinking maybe a kalman is not what is needed. Both location streams are fully matured to their best guess.

Lets say at every second the GPS offers its best guess and the IMU does the same.

At the start, the GPS gets a fix and forwards the same coordinates to the IMU.
The IMU is extremely accurate in calculating the coordinates for the next second.
The GPS again offers its new fix for this next second .... but lets say it produced its worst case guess and is off by 3ft.
After another second, the GPS again offers its fix .... but lets say it produced a guess only 6 inches off true this time.

Would the method of simply summing up the differences between (ie: using the IMU as a anchor reference) the two streams afford an half error improvement?

The IMU retains this summation , then re-calculates for the next second .....bounding the future GPS error swing

[Rick19468]

..... the IMU lives for only one second at a time .... the app uses the summation XY as its working coordinates.

GPS XY difference IMU XY = working XY coordinate

[Nominal Animal]

I guess my last suggestion was very badly explained. Let me try again.

Let's start by turning the device on. The very first GPS measurement gives us gps0_x and gps0_y.

If you don't move, then the best thing to do is to average the GPS measurements, right? For example,

gps0_x = (19 * gps0_x + new_gps_x) / 20;
gps0_y = (19 * gps0_y + new_gps_y) / 20;

which uses 5% weight for the new measurement, 4.75% for the one before that, 4.5125% before that, and so on (100% * 1/20 * (19/20)ⁿ).

That only works if you don't move. If you move, the averaged location will lag after you, changing only very slowly. You can do a test program to see how it behaves; but suffice it to say, if you move suddenly by just one unit, it takes 13 steps to get the averaged value to move by half, 27 steps to move by three quarters, and 89 steps to get closer than 1/100.

So, instead, let's average the origin of our MPU-9150 coordinate system. That does not move, so we can average that! It should work perfectly, because while the GPS has noise, it will not drift; and where MPU-9150 has little noise, it will drift. This picks the best of the two!

First, we need to keep track of the cumulative movement from the origin as reported by MPU-9150. It does not matter at all where the origin is, as long as the coordinates do not wrap. Let us call them mpu_x and mpu_y.

The actual location at any given point in time is then x = gps0_x + mpu_x, y = gps0_y + mpu_y.

(If you want, you can at any time reset the origin, using gps0_x += mpu_x; mpu_x = 0; gps0_y += mpu_y; mpu_y = 0;. It really does not matter!)

Now, assume we get an interrupt when the GPS has measured the signals, and starts calculating. We grab a snapshot of the MPU coordinates then: mpu_old_x = mpu_x and mpu_old_y.

When we receive the GPS coordinate result gps_x, gps_y, we know it should match gps0_x + mpu_old_x, gps0_y + mpu_old_y, right? If there was no noise.

Instead of writing gps_x ≃ gps0_x + mpu_old_x, we can note that gps0_x ≃ gps_x - mpu_old_x -- which should obviously make sense: the origin is where the GPS reports, minus the MPU-9150 -reported small-scale movement!

That means we can now trivially adjust the origin coordinates the same way we did when we were stationary:

gps0_x = (19 * gps0_x) / 20 + (gps_x - mpu_old_x) / 20;
gps0_y = (19 * gps0_y) / 20 + (gps_y - mpu_old_y) / 20;

The reason I used mpu_old_x above is that I think GPS modules do the measurements fast, but spend a lot of time doing the computation. If we used the current coordinates, we'd be off by the amount we have moved after the GPS module started the calculations. If your GPS module does not provide that info, you can use a timer interrupt in the MSP430 to trigger a predefined time after the last GPS report, and just estimate; I'd start with 20% into the measurement cycle.

Is this any better?

[Rick19468]

Thank you for your thoughts on this Nominal Animal! "Is this any better?" .... yes! ......but I'm confused!

Let me ask you this,

You do realize the IMU has NO ability to acquire a "fix" on its own? ..... a "fix" must be fed to it from the GPS right?

Is your approach include trying to "profile" the drift and double integration errors in the IMU with a kalman? ..... you realize the errors go off the charts only after a few seconds time and are cumulative?

When you say "noise" ... this means the reported XY output of the GSP every second?





Is my approach not valid? .... and why?
I would think that refreshing (sampling a new GPS XY "Fix") for the IMU every second, and dead reckoning for only one second max. ..... I need not have to deal with the long term (several seconds) drift and integration errors.

[Nominal Animal]

You do realize the IMU has NO ability to acquire a "fix" on its own? ..... a "fix" must be fed to it from the GPS right?

Exactly. The fix is literally the gps0_x, gps0_y coordinates above. It does not matter where the fix is, as long as it stays in the same physical location. You can even move the fix if you need to (the middle paragraph, the one in parentheses, in my previous post).

Is your approach include trying to "profile" the drift and double integration errors in the IMU with a kalman? ..... you realize the errors go off the charts only after a few seconds time and are cumulative?

It is not a Kalman filter, just a simple averaging filter, but the purpose is the same: to compensate for the IMU drift with the GPS average.

When you say "noise" ... this means the reported XY output of the GSP every second?

Exactly. If you waited stationary for ten seconds, and marked down the GPS readings, they'd look like a shotgun blast targeted at the correct point, right? Wait longer, and you'd have marks most near the correct point, fewer as you go outwards from the point, but basically randomly scattered.

Some GPS modules, I believe, do internal averaging and report the coordinates in larger steps, in an effort to hide that noise. Their shot pattern would be just nine points in a grid: the center point, and the eight nearest points. Such modules are difficult to use for this, because their output does not have noise, but is aliased or quantized; not smooth, changes in steps. Such a GPS module would require a different approach.

I would think that refreshing (sampling a new GPS XY "Fix") for the IMU every second, and dead reckoning for only one second max. ..... I need not have to deal with the long term (several seconds) drift and integration errors.

Your precision would then be limited to the GPS precision, and you'd only use the IMU to interpolate the location between GPS reports. If the IMU drift in one second is larger than the GPS precision, then your precision is even worse. Aside from those limitations, your approach should work.

If the IMU drift is smaller, say less than one half of the GPS precision per GPS measurement interval, then using the filtered GPS to provide the fix for the IMU should yield better than the GPS precision. I assumed this is the case. The lesser the IMU drift compared to GPS, the better the end precision.

The 5% (1/20) factor I used in my example is pretty extreme; it assumes almost no IMU drift. For a big IMU drift, something like 33% to 50% would work much better. If the IMU drift is dependent on acceleration and/or velocity in a simple way, then it should be possible to use a Kalman filter that uses those too to rely more on GPS when drift is large, more on IMU when drift is small. If the dependence is complex, then a Kalman filter won't do, but you might develop a specialized filter to do that. The math is a bit weird, but not too difficult. (I never do that stuff by hand, I always use Maxima or Maple.)

Do you have a development kit for the GPS and IMU you can connect to a computer to get just the measurement data in real time? Or do you know how large the IMU drift may be in practice, per second (GPS report interval)? Since I really have no idea on the practical properties of your hardware in your use case, all my numbers are based on the Stetson-Harrison method.

[Rick19468]

Thank you so much Nominal Animal !


Only now am I getting to understand your method! (I'm sorry, ... you do have the patience of a saint in deed!) : )



What is troubling me with your method is the length of your sampling window and the then needed correction to offset the IMU drift.
You state "(If you want, you can at any time reset the origin, using gps0_x += mpu_x; mpu_x = 0; gps0_y += mpu_y; mpu_y = 0;. It really does not matter!)"
When you say "it really doesn't matter when" this sounds like it doesn't matter how big the averaging window is .... "I have the drift "profiled "and will correct for it."
Is this what your stating? For your method to work .... you must compensate for the drift first, ... to get a good result.



After 5 or so seconds ..... one would have to compensate for the drift, because the IMU goes south very fast after this.


With my method , I use the IMU for the shortest possible time, (with the GPS capable of 5 reads per sec. for 2 dimensions) ....... I read and refresh the IMU at this rate as well.


My greatest need for accuracy is in the short term (5-10 seconds) ...when the dog is standing there .... then bolts to the fence very fast.



C coding for these real time events, and accounting for the lost time for the apps MCU to do calculations will probably require me to read then load both streams first (ie: get them both at the starting line at the same time)... then read the data at cycles end. So, ... the fastest I can read/compare will be around 2.5 times per second.

I'm thinking that having the IMU re-freshed before it can drift of any significance ...... I need not have to address IMU drift at all.

[Nominal Animal]

What is troubling me with your method is the length of your sampling window and the then needed correction to offset the IMU drift.

Ah, I see.

There is really no sampling "window" for the GPS coordinate filter, because the weights follow a simple power law -- mathematically, it extends to infinity --, but you control the same effect by adjusting the weight of the new GPS coordinates.

If the weight factor is w = 5% = 0.05, then the weight of the sample n time steps ago is w (1-w)ⁿ. No, it does not tell me much either, but if you have Gnuplot, then you can plot the sample window for the last n (say 100) steps, relative to the weight of the newest sample, using

gnuplot -p -e 'w = 0.05; n = 100 ; plot [0:n] (1-w)**x notitle w lines'

To calculate some window width, you must state where the cutoff is. For example, the step where the weight is less than half the original step, is -log(2)/log(1-w). The step where the weight drops to 1/10th of the original step, is -log(10)/log(1-w), and so on. I would not bother with that either, I'd just draw those into the graph, and decide it visually:

gnuplot -p -e 'w = 0.05; n = 100 ; plot [0:n] (1-w)**x notitle w lines, 0.5 notitle w lines, 0.1 notitle w lines'

(With weight 5% = 0.05, the sampling window is of the order of fourty to eighty time steps. With weight 15% = 0.15, maybe twenty to thirty steps. With weight 33% = 0.33, maybe a dozen steps. With weight 50% = 0.5, just a few steps.)
_ _ _ _

Resetting the fix point, the origin for the IMU coordinates, is a completely separate thing. It has no relation to the averaging above.

Because the IMU coordinates are by definition relative -- there is no absolute point or fix they refer to, just movement --, we need a reference point, the fix point. The gps0_x and gps0_y provide just that.

The fix point can be freely moved just after we have updated the GPS fix but not prepared for the next GPS sample. It really is nothing more than moving the GPS reference point to somewhere else. And its physical location does not matter, it is just a reference point for the IMU coordinates.

In fact, you can quite safely have your coordinate system work like this:

// Boot-up.
// Get first GPS coordinates into gps_x and gps_y

// Since we have no other samples, we start
// by relying on the initial GPS reading.
gps0_x = gps_x
gps0_y = gps_y

// Weight number N defines the weight factor,
// weight factor w = 1/N
N = 5

// Operation loop:

    // ...
    // Other work .. coordinates are gps0_x + imu_x, gps0_y + imu_y
    // ...

    // Receive signal, or estimate using a timer, that the GPS unit
    // has sampled the location and has started calculations.
    // We use the IMU coordinates at that moment for reference.
    imu_ref_x = imu_x
    imu_ref_y = imu_y

    // ...
    // Other work .. coordinates are gps0_x + imu_x, gps0_y + imu_y
    // ...

    // Receive new GPS coordinates gps_x, gps_y
    // Update fix coordinates,
    gps0_x = ((N-1)*gps0_x + gps_x - imu_ref_x) / N
    gps0_y = ((N-1)*gps0_y + gps_y - imu_ref_y) / N

    // and move fix location so IMU coordinates
    // stay as near zero as possible.
    gps0_x = gps0_x + imu_ref_x
    gps0_y = gps0_y + imu_ref_y
    imu_x = imu_x - imu_ref_x
    imu_y = imu_y - imu_ref_y
    // Note: the above does not affect the actual
    // coordinates, gps0_x+imu_x, gps0_y+imu_y, at all.
    // It really only moves the fix location.

    // ...
    // Other work .. coordinates are gps0_x + imu_x, gps0_y + imu_y
    // ...

With my method , I use the IMU for the shortest possible time, (with the GPS capable of 5 reads per sec. for 2 dimensions) ....... I read and refresh the IMU at this rate as well. My greatest need for accuracy is in the short term (5-10 seconds) ...when the dog is standing there .... then bolts to the fence very fast.

Given an absolute top speed of about 20 m/s, it means the coordinates may change by as much as 4 m (thirteen feet) per one measurement interval (0.2 seconds). Less for normal pets, of course.

It sounds to me you could easily rely on just the GPS for these cases. Given a 0.2 second measurement interval, even an average pet may sprint several feet per interval.

I would personally use much more rapid IMU rate, in the hopes of shortening the reaction time. A large sampling weight (small N in the pseudocode above, maybe 2 to 5), would mean IMU drift would be limited to the drift that accumulates in about one second.

Don't you have hardware you can test this on? I don't mean your final hardware, but a development kit with the same sensors you can read to your computer? Robust enough to swing at the end of a cord? Maybe strap it (data via wifi?) on the back of an unsuspecting dog, camera in the ceiling, then play time. Compare the measured trajectory to the video. Adjust the factors and timings, and repeat. That's what I'd do, anyway.

[Rick19468]

@Nominal Animal Thank-you!

I will be ordering the GPS module and the IMU module soon with an RF module linked back to my pc.

Up to this point I was just pondering if both modules could be made complement each other.

I will certainly try out all your great suggestions!



"I would personally use much more rapid IMU rate"

The IMU can sample up to 2000 dps and has a digital input on fsync pin to support the GPS.


In summary,
Do I understand your method to be the averaging of the two data streams without regard for "profiling" the IMU drift? ( I hope so!) : )

[Nominal Animal]

I will be ordering the GPS module and the IMU module soon with an RF module linked back to my pc.

Sounds good!

It's good to explore the theory and ideas, but when developing gadgets, nothing beats hands-on experience.

(Also, if you choose to put the boundary area within the user-defined polygon, I think I have an algorithm that you can use to shrink the safe area from the warning area boundary. It only fails if you have corridors narrower than twice the warning area. Using only polygons would be certainly fast -- I suspect even checking the polygons a thousand times a second would not be a problem for your MCU. Should we start a new thread, "shrinking a concave polygon"?)

The IMU can sample up to 2000 dps

If you assume a maximum speed of 20 m/s (66 feet/s), then you'd need a rate of say 100 samples per second to be able to sound the alarm within 1 foot of the boundary.

If the sample rate is slower, then the distance a dog can move between samples is correspondingy larger. I think giving immediate, consistent response to canines is absolutely crucial for a gadget like this.

Do I understand your method to be the averaging of the two data streams without regard for "profiling" the IMU drift? ( I hope so!) : )

Well, no..

My method is to use the GPS to provide a global fix and to compensate for the IMU drift.

You don't need to profile the IMU drift, just estimate the combination of maximum IMU drift and GPS measurement noise, to find the best weight factor for each new GPS sample.

There are not that many choices, either; if you use simple integer math, then you just need to choose between N = 2, 3, 4, and maybe 5 (giving you weight factor w=1/N). You can fine-tune it further, but unless you can get the exact specs for both the GPS (noise) and the IMU (drift), you'll have to just try it in practice and see which weight factor works best.

Larger N (smaller weight factor w) rely on IMU and compensate for GPS noise, smaller N (larger weight factor w) rely on GPS and compensate for IMU drift. It's a balance between the two you need to find.