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.
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.
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.
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
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 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
Any software I post in these forums written by me is provided "AS IS" without warranty of any kind, expressed or implied, and permission is hereby granted, free of charge and without restriction, to any person obtaining a copy. To all, peace and happiness.
Reply With Quote
Originally Posted by wikipedia
