Kalman Filter for XY Streams

This is a discussion on Kalman Filter for XY Streams within the Tech Board forums, part of the Community Boards category; "Should we start a new thread, "shrinking a concave polygon"? - NA .... Yes! That would be great! .... but ...

  1. #16
    Registered User
    Join Date
    Oct 2012
    Posts
    62
    "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.
    Last edited by Rick19468; 10-20-2012 at 07:27 PM.

  2. #17
    Registered User
    Join Date
    Oct 2011
    Posts
    826
    Quote Originally Posted by Rick19468 View Post
    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.

    Quote Originally Posted by Rick19468 View Post
    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.

    Quote Originally Posted by Rick19468 View Post
    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.

  3. #18
    Registered User
    Join Date
    Oct 2011
    Posts
    826
    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.
    Code:
    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.

  4. #19
    Registered User
    Join Date
    Oct 2012
    Posts
    62
    @ Nominal Animal : I downloaded Python and PyQt4 ..... where am I to run your code? .... in Python or PyQt4? (Python says "can't find module")

  5. #20
    Registered User
    Join Date
    Oct 2012
    Posts
    62
    "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?
    Last edited by Rick19468; 10-21-2012 at 04:15 PM.

  6. #21
    Registered User
    Join Date
    Oct 2011
    Posts
    826
    Quote Originally Posted by Rick19468 View Post
    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.

    Quote Originally Posted by Rick19468 View Post
    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 4039′51″N 7356′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.)

  7. #22
    and the hat of wrongness Salem's Avatar
    Join Date
    Aug 2001
    Location
    The edge of the known universe
    Posts
    32,451
    Moved to tech forum while the discussion is all about theory rather than actual C++ implementation.
    If you dance barefoot on the broken glass of undefined behaviour, you've got to expect the occasional cut.
    If at first you don't succeed, try writing your phone number on the exam paper.
    I support http://www.ukip.org/ as the first necessary step to a free Europe.

Page 2 of 2 FirstFirst 12
Popular pages Recent additions subscribe to a feed

Similar Threads

  1. Fir filter
    By kiros88 in forum C Programming
    Replies: 3
    Last Post: 06-18-2010, 03:02 PM
  2. Median filter help
    By JTEK24 in forum Tech Board
    Replies: 10
    Last Post: 07-16-2009, 06:05 PM
  3. Gabor Filter in C#
    By nanang in forum C# Programming
    Replies: 0
    Last Post: 05-17-2009, 08:22 PM
  4. How would I filter out non-int entries?
    By rakan in forum C++ Programming
    Replies: 4
    Last Post: 09-20-2006, 01:27 PM
  5. spam filter
    By fnoyan in forum Linux Programming
    Replies: 4
    Last Post: 11-14-2003, 01:03 PM

1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21