Kalman Filter for XY Streams

[Rick19468]

"Should we start a new thread, "shrinking a concave polygon"? - NA

.... Yes! That would be great! .... but it must cover both a mix of convex and concave.


--


"You don't need to profile the IMU drift, just estimate the combination of maximum IMU drift and GPS measurement noise" - NA

Here is were I keep losing you! Why do we need to account for IMU drift at all? You give me the impression that you will be compensating for drift over a long time (>2-3 seconds). The maximum IMU drift would only be for one second max! ..... negligible.


Also, you give me the impression that the GSP is the more accurate position authority. The GPS could be off 3-4 feet at any given time and for a long time (several seconds). Where as the IMU , given a fix to start, is much more accurate in the under one second output frame.


IE: IMU calculates the next position very accurately for the next second after fix, GPS outputs for this reading as well but has errored by 3 feet!

This is why I wanted to use the IMU out to simply "bound the future GPS error swing" .... ​... by simply taking the distance (halve way) between the two (IMU and GPS) best guesses..... halving any error .... and use this for the working position.

[Nominal Animal]

Why do we need to account for IMU drift at all?

It is just the method I believe would yield best results.

Consider the method where you'd just take the GPS coordinates, and only use IMU to estimate the movement from the last GPS point between GPS samples. Your coordinates will be off by the GPS positioning error, plus the IMU error. It's the simplest to implement, but has maximum error.

Because GPS gives you absolute coordinates, and IMU only relative movement since the last movement, you cannot just average the two. If you do, you get nonsense results. And after you fix it to give sensible results, you find you have the previous solution, except you only consider a fraction of the movement the IMU reports between GPS samples.

Also, you give me the impression that the GSP is the more accurate position authority. The GPS could be off 3-4 feet at any given time and for a long time (several seconds). Where as the IMU , given a fix to start, is much more accurate in the under one second output frame.

Not intended. GPS is absolute, IMU relative, positioning information source. They're different, but I wanted each to compensate for the weaknesses in the other.

My method averages multiple GPS samples together to compensate for the GPS noise. To do that, it relies on the IMU to estimate the relative movement between GPS samples. The longer the "averaging window" (the smaller the GPS sample weight), the more the IMU drift will affect the result, because we use the IMU to estimate the movement between samples. The effect is not direct, it just "creeps in" indirectly. Thus, you need to know/guess/estimate, where the balance between IMU and GPS is, by adjusting the GPS sample weight.

Why so complicated? Because you cannot just take GPS samples, and average them together. If you move, that will give you a position that lags behind. And like I said before, you cannot take the average of relative and absolute coordinates, and expect something meaningful.

I don't know how to explain it any better than that, sorry.

This is why I wanted to use the IMU out to simply "bound the future GPS error swing" by simply taking the distance (halve way) between the two (IMU and GPS) best guesses

But IMU gives you relative coordinates, and GPS absolute coordinates. How do you do that, exactly?

Assume you have last GPS coordinates in gps_old_x, gps_old_y, new GPS position in gps_new_x, gps_new_y, and the movement IMU thinks happened between the two GPS measurements, imu_x, imu_y. How do you calculate the new coordinates?

It is easy to calculate an estimate of relative movement between the two (because you can always calculate the relative difference between two absolute positions), but that will suffer from drift. Getting reliable absolute positioning is the hard part.

I know this may be frustrating. I know you feel sure your "want" would work well, but the truth is, it is not rational. It is just a desire. Computers do not care about desires, they only work on math/code/logic. You need to be able to put your plan into hard math (or code), express it in the rigid confines of logic -- otherwise it belongs to the land of daydreams. (I'm a benign dictator in mine.)

At this point, have you considered writing a dog simulator? Create a list of points, say every 1/100th of a second, for a few seconds, then create the GPS and IMU readings, with suitably random noise thrown in, and see how well different methods of combining the GPS and IMU readings match the actual points. Without hardware, that'd be my second favourite option to go forward.

[Nominal Animal]

If anyone is interested, here is a Python + PyQt4 simulator for my method.

I would normally sleep on it, then check the logic, to catch any brainfarts; as it is, it is as likely to be junk as useful.

However, since I only did it for fun, I'll just show it here to get you started. Feel free to do anything with it you want, just don't blame me if something goes wrong.

The various constants are listed at the beginning, and when run, the dancing blobs are described in standard output.

import sys
import random
from PyQt4 import QtGui, QtCore

# Number of GPS samples per second
GPSRATE = 5

# Number of IMU samples per second
IMURATE = 25

# Weight of the latest GPS sample
WEIGHT = 1.0 / 4.0

# Standard deviation of the GPS error
GPSERROR = 20.0

# Standard deviation of the IMU absolute error
IMUABSERROR = 1.5

# Standard deviation of the IMU scaling error
IMURELERROR = 0.1

class GPS(QtCore.QTimer):

    def __init__(self, parent, interval):
        global GPSERROR
        QtCore.QTimer.__init__(self)
        self.Target = parent
        self.Radius = GPSERROR
        self.Position = ( 0, 0 )
        QtCore.QObject.connect(self, QtCore.SIGNAL("timeout()"), self.Signal)
        self.start(int(0.5 + 1000.0 * interval))

    def Signal(self):
        # Generate Gaussian random noise
        noise = ( random.gauss(0.0, self.Radius), random.gauss(0.0, self.Radius) )
        # Use parent position plus noise to generate new position.        
        self.Position = ( int(self.Target.Position[0] + noise[0]), int(self.Target.Position[1] + noise[1]) )
        # Tell parent to repaint.
        self.Target.GPSSignal()


class IMU(QtCore.QTimer):

    def __init__(self, parent, interval):
        global IMUABSERROR
        global IMURELERROR
        QtCore.QTimer.__init__(self)
        self.Target = parent
        self.TargetLast = parent.Position
        self.Position = ( 0, 0 )
        self.Radius = IMUABSERROR
        self.Scale = IMURELERROR
        QtCore.QObject.connect(self, QtCore.SIGNAL("timeout()"), self.Signal)
        self.start(int(0.5 + 1000.0 * interval))

    def Signal(self):
        # Calculate true movement since last signal
        currTarget = ( self.Target.Position[0], self.Target.Position[1] )
        prevTarget = ( self.TargetLast[0], self.TargetLast[1] )
        self.TargetLast = ( currTarget[0], currTarget[1] )
        delta = ( currTarget[0] - prevTarget[0], currTarget[1] - prevTarget[1] )

        # Generate Gaussian random noise for absolute error,
        noise = ( random.gauss(0.0, self.Radius), random.gauss(0.0, self.Radius) )
        # and for scaling error.
        scale = random.gauss(1.0, self.Scale)

        # Calculate simulated delta,
        self.Delta = ( noise[0] + scale * delta[0], noise[1] + scale * delta[1] )
        # and add to current position.
        self.Position = ( self.Position[0] + self.Delta[0], self.Position[1] + self.Delta[1] )

        # Update.        
        self.Target.IMUSignal()


class Dog(QtGui.QMainWindow):

    def __init__(self, *args):
        global GPSRATE
        global IMURATE

        apply(QtGui.QMainWindow.__init__, (self,) + args)

        # Set the window title and margins.
        self.setWindowTitle('Dog')
        self.setContentsMargins(0, 0, 0, 0)

        # Define a new palette, with black button/text color, and a light gray background.
        self.setPalette(QtGui.QPalette(QtGui.QColor(0,0,0,255), QtGui.QColor(204,204,204,255)))
        self.setBackgroundRole(QtGui.QPalette.Base)

        # We need to track mouse movement.
        self.setMouseTracking(True)

        # Initial Dog position.
        self.Position = ( 0, 0 )

        # Initial GPS FIX location.
        self.Fix = ( 0, 0 )

        # Simulate a GPS,
        self.GPS = GPS(self, 1.0 / GPSRATE)

        # and an IMU.
        self.IMU = IMU(self, 1.0 / IMURATE)

        # Update.
        self.repaint()

    def GPSSignal(self):
        # Update GPS fix location.
        self.Fix = ( (1.0 - WEIGHT) * self.Fix[0] + WEIGHT * (self.GPS.Position[0] - self.IMU.Position[0]), (1.0 - WEIGHT) * self.Fix[1] + WEIGHT * (self.GPS.Position[1] - self.IMU.Position[1] ) )
        self.repaint()

    def IMUSignal(self):
        self.repaint()

    def paintEvent(self, event):
        global WEIGHT

        # Handle for drawing on our window.
        painter = QtGui.QPainter()

        # Bottom right corner and center of the window.
        limits = ( self.width(), self.height() )
        center = ( limits[0] / 2, limits[1] / 2 )


        painter.begin(self)

        # Clear to background color.
        painter.fillRect(self.geometry(), QtGui.QBrush(QtGui.QColor(204, 204, 204, 255)))

        # Draw the centerlines.
        painter.setBrush(QtCore.Qt.NoBrush)
        painter.setPen(QtGui.QColor(153, 153, 153, 255))
        painter.drawLine(center[0], 0, center[0], limits[1])
        painter.drawLine(0, center[1], limits[0], center[1])

        # Draw the GPS Fix, a small gray blob.
        painter.setBrush(QtGui.QColor(0, 0, 0, 51))
        painter.setPen(QtGui.QColor(153, 153, 153, 255))
        painter.drawEllipse(QtCore.QPoint(center[0] + self.Fix[0], center[1] + self.Fix[1]), 4, 4)

        # Draw the GPS point, a green blob.
        painter.setBrush(QtGui.QColor(0, 255, 0, 51))
        painter.setPen(QtGui.QColor(0, 153, 0, 255))
        painter.drawEllipse(QtCore.QPoint(center[0] + self.GPS.Position[0], center[1] + self.GPS.Position[1]), 5, 5)

        # Draw the IMU point, a blue blob.
        painter.setBrush(QtGui.QColor(0, 0, 255, 51))
        painter.setPen(QtGui.QColor(0, 0, 204, 255))
        painter.drawEllipse(QtCore.QPoint(center[0] + self.IMU.Position[0], center[1] + self.IMU.Position[1]), 5, 5)

        # Draw the estimated point, a red blob
        point = ( self.IMU.Position[0] + self.Fix[0], self.IMU.Position[1] + self.Fix[1] )
        painter.setBrush(QtGui.QColor(255, 0, 0, 102))
        painter.setPen(QtGui.QColor(153, 0, 0, 255))
        painter.drawEllipse(QtCore.QPoint(center[0] + point[0], center[1] + point[1]), 8, 8)
        # and a line to the actual point.
        painter.drawLine(center[0] + point[0], center[1] + point[1], center[0] + self.Position[0], center[1] + self.Position[1])

        # Draw the Dog point, black and white.
        painter.setBrush(QtGui.QColor(255, 255, 255, 51))
        painter.setPen(QtGui.QColor(0, 0, 0, 255))
        painter.drawEllipse(QtCore.QPoint(center[0] + self.Position[0], center[1] + self.Position[1]), 5, 5)

        painter.end()


    def mousePressEvent(self, event):
        self.Position = ( event.x() - self.width() / 2, event.y() - self.height() / 2 )
        self.repaint()


    def mouseMoveEvent(self, event):
        if event.buttons() == QtCore.Qt.LeftButton:
            self.Position = ( event.x() - self.width() / 2, event.y() - self.height() / 2 )
        self.repaint()

if __name__ == '__main__':
    print ""
    print "The black and white circle is the dog. Click or drag to move."
    print "The red blob is the estimated position, with line showing the error."
    print "The blue blob describes cumulative IMU coordinates."
    print "The green blob describes GPS points."
    print "The small gray blob describes the GPS fix location."
    print ""
    application = QtGui.QApplication(sys.argv)
    window = Dog()
    window.show()
    window.resizeEvent(None)
    result = application.exec_()
    sys.exit(result)

It should run on any OS, as long as you have Python and PyQt4 installed (they are available to just about all OSes, certainly for Linux, Mac OS X, and Windows). Save the above to dog.py, then run it with python dog.py .

Click or drag to move the "dog" around.

[Rick19468]

@ Nominal Animal : I downloaded Python and PyQt4 ..... where am I to run your code? .... in Python or PyQt4? (Python says "can't find module")

[Rick19468]

"Assume you have last GPS coordinates in gps_old_x, gps_old_y, new GPS position in gps_new_x, gps_new_y, and the movement IMU thinks happened between the two GPS measurements, imu_x, imu_y. How do you calculate the new coordinates?"

I was under the impression that I could have the IMU "mirror" (for input) the GPS coordinates either directly or scaled to reflect the same coordinate structure.

From there, we have two positions offered, GPS and IMU. Draw a line between the two and calculate the halve way distance between the two.

but, your stating the IMU cannot be scaled or mirrored to real (absolute) coordinates?

[Nominal Animal]

where am I to run your code?

If you have 32-bit Windows, use the Python 2.7.3 installer to install Python, then download and run the PyQt4 installer for Python 2.7.

If you have 64-bit Windows, use the 64-bit Python 2.7.3 installer to install Python, then download and run the 64-bit PyQt4 installer for Python 2.7.

After that, you only should need to save my file as dog.py, and double-click on it to run it. I don't use Windows, so if you're having trouble, I suggest you head over to the python.org wiki.

If you use Linux, just install the python and pyqt packages in your package management, then enter the directory you put the dog.pyin, and run python dog.pythere. Or add line #!/usr/bin/python at the start of the file, and make it executable, so you can just double-click on it.

I was under the impression that I could have the IMU "mirror" (for input) the GPS coordinates either directly or scaled to reflect the same coordinate structure. From there, we have two positions offered, GPS and IMU. Draw a line between the two and calculate the halve way distance between the two.
but, your stating the IMU cannot be scaled or mirrored to real (absolute) coordinates?

Nope, not possible. You need a fix point, an origin, to convert relative coordinates (like IMU coordinates) to real-world, absolute coordinates (like GPS coordinates).

GPS gives you world coordinates. IMU is more like an odometer; it will only tell you how far and in what direction you have moved since it was turned on (or reset), or since last report: for example, "5 feet north, 3 feet east, since the last time you asked".

What you are describing is somehow drawing a line between 40°39′51″N 73°56′19″W and 15 feet north, 7 feet west, and marking a point half way. It just does not make any sense.

(If you just add the two together, maybe scaling the relative coordinates (feet) somehow, you get the worst combination possible: GPS errors PLUS IMU errors. The GPS coordinates are not filtered in any way. You rely completely on the GPS readings, and just add whatever the IMU reports on top of that, maybe scaled down. It is not averaging, you're just adding (scaled) IMU coordinates to GPS coordinates.)

[Salem]

Moved to tech forum while the discussion is all about theory rather than actual C++ implementation.