Kalman Filter

I was very surprized when I searched on the forums for "kalman" and no threads were found.

I bought an Triple Axis Accelerometer with Triple Axis Gyroscope, I managed to read the values in the computer from the device using the comm port.

The problem is that I get ~ 390 frames per second, and the numbers vary a lot. Even when I don't move the device, the numbers still change a lot. I think I need to average the values (or something like that). But searching on the net, I found that this is best done with a Kalman Filter.

The best example of what I need to achive is shown here:
http://www.youtube.com/watch?v=AWAFFZ7rPDc

If you look at the right, that is what I get right now, and what I need is on the left.

I wish I knew how to explain this a little better.

Does anyone have any experience with this ?

Whats he building in there?

How about posting some sample data? I'm not sure I can help but there no harm in trying. What about 5 seconds of no movement and a series of 5 second chunks with different defined movements.

Quote:

---

Originally Posted by wikipedia

The Kalman filter is an efficient recursive filter that estimates the state of a dynamic system from a series of incomplete and noisy measurements. Together with the linear-quadratic regulator (LQR) the Kalman filter solves the linear-quadratic-Gaussian control problem (LQG). The Kalman filter, the linear-quadratic regulator and the linear-quadratic-Gaussian controller are solutions to what probably are the most fundamental problems in control theory.

---

Wow, sounds challenging.

Thanks for your reply Milk,

What am I trying to build ? well, I am trying to make a balancing robot, like this one:
http://www.youtube.com/watch?v=3hJyiDPR9Gw

I attached some text files in a zip, in CSV format with some readings I took for 5 seconds each...

By the way, the readings are 12 bit, so the highest value should be 1024, but I don't think the board gets exactly 5 Volts that's why in my readings the highest value is 1008.

What type of micro controller are you using??
I am working in the same project right now? HOV far are you to be done.
Kalman has to be implemented in the Micro controller C code..
I will post the link where you can find the kalman libraries in C++.. the ones from your youtube link ..
http://tom.pycke.be/mav/92/kalman-demo-application

Hi thiever, welcome to the forums :wave:

Unfortunately, I was not able to work on my robot for quite some time.

The link you provided is really good. I downloaded the code, and it looks very promising.

When I will have time, the first thing I will do, is plug the 6DOF (the accelerometer and gyro) into my computer, and I will make a program (probably in VB6) that will do the same thing as in the video.

After that, when I know all the coding and logic, then I will transfer the logic to the microcontroller.

I'm new in imu manipulation and I bought a sparkfun 6dof V4. I want to know what's the meaning of the output data.
ie: magX : 626

is it 626 mV or 626 deg radian.

I don't know how to convert these data in angle to georeference aerial photography.

Thanks

Hey michael,

i was wondering if you figured out how to do kalman filter in visual basic. I have a project that requires doing so but and it will really be helpful if i follow an open source code if you found any ?

Hmm, out of curiousity, I looked around, and nothing seems to be available. I've actually got a few other noise/signal type filters available in VB6, but no Kalman. I did a bit of research on it, and it's going to take a bit of doing to do it correctly. There seem to be a few Fortran or C (or C++ or C#) versions around. I suppose you could translate one of those. However, to do it correctly, it appears you need the inverse-normal(Gauss)-distribution function. The following is a version of Gaussian distributions (leaning heavily on the ALGLIB project to get it done). It's specifically the NormalCdfInv function that you'll need.

Code:

---


Option Explicit
'
Const e        As Double = 2.71828182845905
Const pi      As Double = 3.14159265358979
Const sqr2    As Double = 1.4142135623731
Const MaxDouble As Double = 1E+300
'

Public Function NormalPdf(x As Double, Optional Mean As Double, Optional SD As Double = 1#) As Double
    NormalPdf = (e ^ ((-(x - Mean) ^ 2#) / (2# * SD ^ 2#))) / (SD * Sqr(2# * pi))
End Function

Public Function NormalCdf(x As Double, Optional Mean As Double, Optional SD As Double = 1#) As Double
    NormalCdf = (1# + Erf((x - Mean) / (SD * sqr2))) / 2#
End Function

Public Function NormalCdfInv(cdf As Double, Optional Mean As Double, Optional SD As Double = 1#) As Double
    ' Cdf should be bounded by 0 and 1.
    '
    Dim x As Double
    '
    x = ErfInv(2# * cdf - 1#)
    If Abs(x) <> MaxDouble Then NormalCdfInv = sqr2 * x * SD + Mean
End Function

Public Function Erf(ByVal x As Double) As Double
    ' The error function related to the Gaussian distribution.
    Dim XSq As Double
    Dim s As Double
    Dim p As Double
    Dim Q As Double
    '
    s = Sgn(x)
    x = Abs(x)
    Select Case x
    Case Is < 0.5
        XSq = x * x
        p = 7.54772803341863E-03
        p = 0.288805137207594 + XSq * p
        p = 14.3383842191748 + XSq * p
        p = 38.0140318123903 + XSq * p
        p = 3017.82788536508 + XSq * p
        p = 7404.07142710151 + XSq * p
        p = 80437.363096084 + XSq * p
        Q = 0#
        Q = 1# + XSq * Q
        Q = 38.0190713951939 + XSq * Q
        Q = 658.07015545924 + XSq * Q
        Q = 6379.60017324428 + XSq * Q
        Q = 34216.5257924629 + XSq * Q
        Q = 80437.363096084 + XSq * Q
        Erf = s * 1.12837916709551 * x * p / Q
    Case Is >= 10#
        Erf = s
    Case Else
        p = 0#
        p = 0.56418778255074 + x * p
        p = 9.67580788298727 + x * p
        p = 77.0816173036843 + x * p
        p = 368.519615471001 + x * p
        p = 1143.26207070389 + x * p
        p = 2320.43959025164 + x * p
        p = 2898.02932921677 + x * p
        p = 1826.33488422951 + x * p
        Q = 1#
        Q = 17.1498094362761 + x * Q
        Q = 137.125596050062 + x * Q
        Q = 661.736120710765 + x * Q
        Q = 2094.38436778954 + x * Q
        Q = 4429.61280388368 + x * Q
        Q = 6089.54242327244 + x * Q
        Q = 4958.82756472114 + x * Q
        Q = 1826.33488422951 + x * Q
        Erf = s * (1# - Exp(-(x * x)) * p / Q)
    End Select
End Function

Public Function ErfInv(ByVal y As Double) As Double
    ' The inverse error function related to the Gaussian distribution.
    Dim Expm2 As Double
    Dim S2Pi As Double
    Dim x As Double
    Dim z As Double
    Dim Y2 As Double
    Dim X0 As Double
    Dim X1 As Double
    Dim code As Boolean
    Dim P0 As Double
    Dim Q0 As Double
    Dim P1 As Double
    Dim Q1 As Double
    Dim p2 As Double
    Dim Q2 As Double
    '
    y = 0.5 * (y + 1#)
    Select Case y
    Case Is <= 0#
        ErfInv = -MaxDouble
    Case Is >= 1#
        ErfInv = MaxDouble
    Case Else
        Expm2 = 1# / Exp(2#)
        S2Pi = Sqr(2# * pi)
        code = True
        If y > 1# - Expm2 Then
            y = 1# - y
            code = False
        End If
        If y > Expm2 Then
            y = y - 0.5
            Y2 = y * y
            P0 = -59.9633501014108
            P0 = 98.0010754186 + Y2 * P0
            P0 = -56.676285746907 + Y2 * P0
            P0 = 13.931260938728 + Y2 * P0
            P0 = -1.23916583867381 + Y2 * P0
            Q0 = 1#
            Q0 = 1.95448858338142 + Y2 * Q0
            Q0 = 4.67627912898882 + Y2 * Q0
            Q0 = 86.3602421390891 + Y2 * Q0
            Q0 = -225.462687854119 + Y2 * Q0
            Q0 = 200.260212380061 + Y2 * Q0
            Q0 = -82.0372256168333 + Y2 * Q0
            Q0 = 15.9056225126212 + Y2 * Q0
            Q0 = -1.1833162112133 + Y2 * Q0
            x = y + y * Y2 * P0 / Q0
            x = x * S2Pi
            ErfInv = x / sqr2
        Else
            x = Sqr(-(2# * Log(y)))
            X0 = x - Log(x) / x
            z = 1# / x
            If x < 8# Then
                P1 = 4.05544892305962
                P1 = 31.5251094599894 + z * P1
                P1 = 57.1628192246421 + z * P1
                P1 = 44.0805073893201 + z * P1
                P1 = 14.6849561928858 + z * P1
                P1 = 2.1866330685079 + z * P1
                P1 = -(1.40256079171354 * 0.1) + z * P1
                P1 = -(3.50424626827848 * 0.01) + z * P1
                P1 = -(8.57456785154685 * 0.0001) + z * P1
                Q1 = 1#
                Q1 = 15.7799883256467 + z * Q1
                Q1 = 45.3907635128879 + z * Q1
                Q1 = 41.3172038254672 + z * Q1
                Q1 = 15.0425385692908 + z * Q1
                Q1 = 2.50464946208309 + z * Q1
                Q1 = -(1.42182922854788 * 0.1) + z * Q1
                Q1 = -(3.80806407691578 * 0.01) + z * Q1
                Q1 = -(9.33259480895457 * 0.0001) + z * Q1
                X1 = z * P1 / Q1
            Else
                p2 = 3.23774891776946
                p2 = 6.91522889068984 + z * p2
                p2 = 3.93881025292474 + z * p2
                p2 = 1.33303460815808 + z * p2
                p2 = 2.01485389549179 * 0.1 + z * p2
                p2 = 1.2371663481782 * 0.01 + z * p2
                p2 = 3.01581553508235 * 0.0001 + z * p2
                p2 = 2.65806974686738 * 0.000001 + z * p2
                p2 = 6.23974539184983 * 0.000000001 + z * p2
                Q2 = 1#
                Q2 = 6.02427039364742 + z * Q2
                Q2 = 3.67983563856161 + z * Q2
                Q2 = 1.37702099489081 + z * Q2
                Q2 = 2.16236993594497 * 0.1 + z * Q2
                Q2 = 1.34204006088543 * 0.01 + z * Q2
                Q2 = 3.28014464682128 * 0.0001 + z * Q2
                Q2 = 2.89247864745381 * 0.000001 + z * Q2
                Q2 = 6.79019408009981 * 0.000000001 + z * Q2
                X1 = z * p2 / Q2
            End If
            x = X0 - X1
            If code Then
                x = -x
            End If
            ErfInv = x / sqr2
        End If
    End Select
End Function


---

And, if you get determined, here's an Excel workbook that walks you through how to do it. If you actually do get this pulled together, please post your results. I'm quite curious to see it in action.

Good Luck,
Elroy