Gyro-Signal (und RC-Empfänger) filtern

[Che Guevara]

Hallo,

nachdem ich nun erfolgreich meinen PID-Regler implementiert habe, möchte ich nun die Gyro-Signale meiner WM+ filtern, um etwas bessere Werte zu erhalten. Aber ich weiß leider nicht so genau, wie ich die Signale filtern soll?!
1. Mit einem Komplementärfilter (also Hochpass und Tiefpassfilter, oder?!)
2. Mit einem Kalmanfilter (ist mir ehrlich gesagt zu kompliziert)
3. Nur mit einem Hochpass bzw. nur mit einem Tiefpassfilter

Was wäre eurer Meinung nach am besten geeignet? Der Tricopter soll jetzt nicht da minutelang in der Luft schweben, aber momentan ist es leider so, dass die Motoren vereinzelt schon im Stand anfangen, sich zu drehen, wegen dem Drift...
Außerdem möchte ich noch einen Filter für den RC-Empfänger implementieren, da diese Werte auch geringfügig schwanken. Welchen Filter sollte ich den hier verwenden?
Hier noch der jetztige Code:


$regfile = "m8def.dat"
$crystal = 16000000
$framesize = 140
$hwstack = 120
$swstack = 120
$baud = 38400


Declare Sub Wmp_init()
Declare Sub Send_zero()
Declare Sub Read_data()
Declare Sub Set_offset()

Declare Sub Filter_gyro_data()
Declare Sub Pid_regulator()
Declare Sub Filter_rx_data()
Declare Sub Mixer()
Declare Sub Send_data()



$lib "I2C_TWI.LBX" 'Hardware I2C
Config Scl = Portc.5 'Ports for I2C-Bus
Config Sda = Portc.4
Config Twi = 100000 '400000
I2cinit


Config Timer1 = Timer , Prescale = 1
On Timer1 Pausedetect
Enable Timer1

Config Int1 = Falling
On Int1 Measure
Enable Int1


Config Pind.3 = Input
Portd.3 = 0


Const Start_byte = 127
Const _maxchannel = 4

Const _bl1offset = 0
Const _bl2offset = 0
Const _bl3offset = 0
Const _bl4offset = -600

Const _yaw_kp = 4
Const _roll_kp = 4
Const _pitch_kp = 4

Const _yaw_ki = 2
Const _roll_ki = 2
Const _pitch_ki = 2

Const _yaw_kd = 5
Const _roll_kd = 5
Const _pitch_kd = 5



Dim _yaw_kp_err As Integer
Dim _roll_kp_err As Integer
Dim _pitch_kp_err As Integer

Dim _yaw_ki_err As Integer
Dim _roll_ki_err As Integer
Dim _pitch_ki_err As Integer

Dim _yaw_ki_sum As Integer
Dim _roll_ki_sum As Integer
Dim _pitch_ki_sum As Integer

Dim _yaw_kd_err As Integer
Dim _roll_kd_err As Integer
Dim _pitch_kd_err As Integer

Dim _yaw_kd_old As Integer
Dim _roll_kd_old As Integer
Dim _pitch_kd_old As Integer

Dim _yaw_pid As Integer
Dim _roll_pid As Integer
Dim _pitch_pid As Integer

Dim _yaw_err As Integer
Dim _roll_err As Integer
Dim _pitch_err As Integer


Dim Bufferbyte As Byte
Dim Kanal(_maxchannel) As Word
Dim Channel As Byte
Dim _bl(_maxchannel) As Word
Dim I As Byte
Dim _crc As Word
Dim _sbl(_maxchannel) As Integer

Dim Buffer(6) As Byte

Dim Yaw As Word
Dim Yaw0 As Byte At Yaw + 1 Overlay
Dim Yaw1 As Byte At Yaw Overlay

Dim Roll As Word
Dim Roll0 As Byte At Roll + 1 Overlay
Dim Roll1 As Byte At Roll Overlay

Dim Pitch As Word
Dim Pitch0 As Byte At Pitch + 1 Overlay
Dim Pitch1 As Byte At Pitch Overlay

Dim _yawoffset As Long
Dim _rolloffset As Long
Dim _pitchoffset As Long

Dim _yawnow As Integer
Dim _rollnow As Integer
Dim _pitchnow As Integer

Dim _empfmiddle(_maxchannel) As Word
Dim _empfmin(_maxchannel) As Word
Dim _empfmax(_maxchannel) As Word
Dim _empfdiv(_maxchannel) As Word
Dim _stick_sensitivy(_maxchannel) As Word

_stick_sensitivy(1) = 2265
_stick_sensitivy(2) = 1800
_stick_sensitivy(3) = 1800
_stick_sensitivy(4) = 1800

_empfmiddle(1) = 26500
_empfmiddle(2) = 23800
_empfmiddle(3) = 25300
_empfmiddle(4) = 22250

_empfmin(1) = 14300
_empfmin(2) = 14650
_empfmin(3) = 17100
_empfmin(4) = 14750

_empfmax(1) = 32300
_empfmax(2) = 32600
_empfmax(3) = 32600
_empfmax(4) = 30500

For I = 1 To 4
_empfdiv(i) = _empfmiddle(i) - _empfmin(i)
_empfdiv(i) = _empfdiv(i) / _stick_sensitivy(i) '2265
_empfdiv(i) = _empfdiv(i) * 2
Next I


Call Wmp_init()
Waitms 500
Call Set_offset()

Wait 2

Enable Interrupts



Do


Call Filter_rx_data()
Call Filter_gyro_data()
Call Pid_regulator()
Call Mixer()
Call Send_data()


Loop


Measure:
If Channel > 0 And Channel < 5 Then
Kanal(channel) = Timer1
End If
Timer1 = 1536
Incr Channel
Return

Pausedetect:
Channel = 0
Return


Sub Wmp_init()
I2cstart
I2cwbyte &HA6 ' sends memory address
I2cwbyte &HFE ' WM+ activation
I2cwbyte &H04 . ' Now Adress changes to &HA4
I2cstop
End Sub

Sub Send_zero()
I2cstart
I2cwbyte &HA4 ' sends memory address
I2cwbyte &H00 ' sends zero before receiving
I2cstop
Waitms 1
End Sub

Sub Read_data()
Gosub Send_zero ' sends zero before receiving
I2creceive &HA4 , Buffer(1) , 0 , 6 ' receive 6 bytes
Yaw1 = Buffer(1)
Roll1 = Buffer(2) ' Low Bytes
Pitch1 = Buffer(3)
Shift Buffer(4) , Right , 2 : Yaw0 = Buffer(4)
Shift Buffer(5) , Right , 2 : Roll0 = Buffer(5) ' High Bytes
Shift Buffer(6) , Right , 2 : Pitch0 = Buffer(6)
End Sub

Sub Set_offset()
_yawoffset = 0
_rolloffset = 0
_pitchoffset = 0
For I = 1 To 100
Call Read_data
_yawoffset = _yawoffset + Yaw
_rolloffset = _rolloffset + Roll
_pitchoffset = _pitchoffset + Pitch
Next I
_yawoffset = _yawoffset / 100
_rolloffset = _rolloffset / 100
_pitchoffset = _pitchoffset / 100
End Sub



Sub Filter_gyro_data()
Call Read_data()
_yawnow = Yaw - _yawoffset
_rollnow = Roll - _rolloffset
_pitchnow = Pitch - _pitchoffset

_yawnow = _yawnow / 2
_rollnow = _rollnow / 2
_pitchnow = _pitchnow / 2
End Sub



Sub Pid_regulator()
_yaw_err = _sbl(4) / 1
_yaw_err = _yaw_err - _yawnow

_roll_err = _sbl(3) / 5
_roll_err = _roll_err - _rollnow

_pitch_err = _sbl(2) / 5
_pitch_err = _pitch_err - _pitchnow


_yaw_kp_err = _yaw_kp_err * _yaw_kp
_yaw_kp_err = _yaw_err / 10

_roll_kp_err = _roll_kp_err * _roll_kp
_roll_kp_err = _roll_err / 10

_pitch_kp_err = _pitch_kp_err * _pitch_kp
_pitch_kp_err = _pitch_err / 10


_yaw_ki_err = _yaw_ki_err * _yaw_ki
_yaw_ki_err = _yaw_err / 50
_yaw_ki_sum = _yaw_ki_sum + _yaw_ki_err

_roll_ki_err = _roll_ki_err * _roll_ki
_roll_ki_err = _roll_err / 50
_roll_ki_sum = _roll_ki_sum + _roll_ki_err

_pitch_ki_err = _pitch_ki_err * _pitch_ki
_pitch_ki_err = _pitch_err / 50
_pitch_ki_sum = _pitch_ki_sum + _pitch_ki_err


_yaw_kd_err = _yaw_kd_err * _yaw_kd
_yaw_kd_err = _yaw_err / 50
_yaw_kd_err = _yaw_kd_old - _yaw_kd_err
_yaw_kd_old = _yaw_kd_old

_roll_kd_err = _roll_kd_err * _roll_kd
_roll_kd_err = _roll_err / 50
_roll_kd_err = _roll_kd_old - _roll_kd_err
_roll_kd_old = _roll_kd_old

_pitch_kd_err = _pitch_kd_err * _pitch_kd
_pitch_kd_err = _pitch_err / 50
_pitch_kd_err = _pitch_kd_old - _pitch_kd_err
_pitch_kd_old = _pitch_kd_old


_yaw_pid = _yaw_kp_err + _yaw_ki_sum
_yaw_pid = _yaw_pid + _yaw_kd_err

_roll_pid = _roll_kp_err + _roll_ki_sum
_roll_pid = _roll_pid + _roll_kd_err

_pitch_pid = _pitch_kp_err + _pitch_ki_sum
_pitch_pid = _pitch_pid + _pitch_kd_err


If _yaw_pid < -1000 Then _yaw_pid = -1000
If _yaw_pid > 1000 Then _yaw_pid = 1000
If _roll_pid < -1000 Then _roll_pid = -1000
If _roll_pid > 1000 Then _roll_pid = 1000
If _pitch_pid < -1000 Then _pitch_pid = -1000
If _pitch_pid > 1000 Then _pitch_pid = 1000
End Sub


Sub Filter_rx_data()
For I = 1 To 4
_sbl(i) = Kanal(i) - _empfmiddle(i)
_sbl(i) = _sbl(i) / _empfdiv(i)
Next I

_sbl(4) = _sbl(4) * -1

_sbl(2) = _sbl(2) / 2
_sbl(3) = _sbl(3) / 2
_sbl(4) = _sbl(4) / 2
End Sub


Sub Mixer()
_bl(1) = 62667 - _sbl(1)
_bl(2) = _bl(1)
_bl(3) = _bl(1)
_bl(4) = 62667

_bl(1) = _bl(1) + _bl1offset
_bl(2) = _bl(2) + _bl2offset
_bl(3) = _bl(3) + _bl3offset
_bl(4) = _bl(4) + _bl4offset

_bl(1) = _bl(1) + _pitch_pid
_bl(2) = _bl(2) + _roll_pid
_bl(3) = _bl(3) - _roll_pid
_bl(4) = _bl(4) - _yaw_pid
End Sub


Sub Send_data()
If Channel <= 11 Then
_crc = Crc16(_bl(1) , 4)
Printbin Start_byte ; _bl(1) ; _bl(2) ; _bl(3) ; _bl(4) ; _crc
End If
End Sub


End
Vielen Dank
Gruß
Chris

EDIT:
Wenn jemand einen Beispielcode oder eine Seite hätte, auf der man ein Beispiel einer Implementierung sieht, wäre ich nicht abgeneigt, da sich das Internet über solche Implementaitionen doch sehr ausschweigt :/
Ein Tiefpass-Filter ist mir eigtl. klar, das ist im Prinzip nichts anderes als eine Durchschnittsbildung.

[Che Guevara]

Hallo,

ich habe jetzt mal einen IIR-Filter für den RC-Empfänger implementiert, das hat schon eine kleine Verbesserung gebracht! Jedoch suche ich immer noch händeringend nach einem Filter für die Gyro-Daten... Soll ichs dort auch mal mit einem IIR probieren?
Hier noch der Code:


$regfile = "m8def.dat"
$crystal = 16000000
$framesize = 140
$hwstack = 120
$swstack = 120
$baud = 38400


Declare Sub Wmp_init()
Declare Sub Send_zero()
Declare Sub Read_data()
Declare Sub Set_offset()

Declare Sub Filter_gyro_data()
Declare Sub Pid_regulator()
Declare Sub Filter_rx_data()
Declare Sub Mixer()
Declare Sub Send_data()


$lib "I2C_TWI.LBX" 'Hardware I2C
Config Scl = Portc.5 'Ports for I2C-Bus
Config Sda = Portc.4
Config Twi = 100000
'400000
I2cinit


Config Timer1 = Timer , Prescale = 1
On Timer1 Pausedetect
Enable Timer1

Config Int1 = Falling
On Int1 Measure
Enable Int1


Config Pind.3 = Input
Portd.3 = 0


Const Start_byte = 127
Const _maxchannel = 4

Const _bl1offset = 0
Const _bl2offset = 0
Const _bl3offset = 0
Const _bl4offset = -600

Const _yaw_kp = 5
Const _roll_kp = 5
Const _pitch_kp = 5

Const _yaw_ki = 3
Const _roll_ki = 3
Const _pitch_ki = 3

Const _yaw_kd = 5
Const _roll_kd = 5
Const _pitch_kd = 5


Dim _yaw_kp_err As Integer
Dim _roll_kp_err As Integer
Dim _pitch_kp_err As Integer

Dim _yaw_ki_err As Integer
Dim _roll_ki_err As Integer
Dim _pitch_ki_err As Integer

Dim _yaw_ki_sum As Integer
Dim _roll_ki_sum As Integer
Dim _pitch_ki_sum As Integer

Dim _yaw_kd_err As Integer
Dim _roll_kd_err As Integer
Dim _pitch_kd_err As Integer

Dim _yaw_kd_old As Integer
Dim _roll_kd_old As Integer
Dim _pitch_kd_old As Integer

Dim _yaw_pid As Integer
Dim _roll_pid As Integer
Dim _pitch_pid As Integer

Dim _yaw_err As Integer
Dim _roll_err As Integer
Dim _pitch_err As Integer


Dim _sbl_filter(4) As Integer
Dim _sbl_filter_2(4) As Integer


Dim Bufferbyte As Byte
Dim Kanal(_maxchannel) As Word
Dim Channel As Byte
Dim _bl(_maxchannel) As Word
Dim I As Byte
Dim _crc As Word
Dim _sbl(_maxchannel) As Integer

Dim Buffer(6) As Byte

Dim Yaw As Word
Dim Yaw0 As Byte At Yaw + 1 Overlay
Dim Yaw1 As Byte At Yaw Overlay

Dim Roll As Word
Dim Roll0 As Byte At Roll + 1 Overlay
Dim Roll1 As Byte At Roll Overlay

Dim Pitch As Word
Dim Pitch0 As Byte At Pitch + 1 Overlay
Dim Pitch1 As Byte At Pitch Overlay

Dim _yawoffset As Long
Dim _rolloffset As Long
Dim _pitchoffset As Long

Dim _yawnow As Integer
Dim _rollnow As Integer
Dim _pitchnow As Integer

Dim _empfmiddle(_maxchannel) As Word
Dim _empfmin(_maxchannel) As Word
Dim _empfmax(_maxchannel) As Word
Dim _empfdiv(_maxchannel) As Word
Dim _stick_sensitivy(_maxchannel) As Word

_stick_sensitivy(1) = 2265
_stick_sensitivy(2) = 1800
_stick_sensitivy(3) = 1800
_stick_sensitivy(4) = 2265

_empfmiddle(1) = 26500
_empfmiddle(2) = 23800
_empfmiddle(3) = 25300
_empfmiddle(4) = 22250

_empfmin(1) = 14300
_empfmin(2) = 14650
_empfmin(3) = 17100
_empfmin(4) = 14750

_empfmax(1) = 32300
_empfmax(2) = 32600
_empfmax(3) = 32600
_empfmax(4) = 30500



For I = 1 To 4
_empfdiv(i) = _empfmiddle(i) - _empfmin(i)
_empfdiv(i) = _empfdiv(i) / _stick_sensitivy(i) '2265
_empfdiv(i) = _empfdiv(i) * 2
Next I


Call Wmp_init()
Waitms 500
Call Set_offset()
Enable Interrupts



Do


Call Filter_rx_data()
Call Filter_gyro_data()
Call Pid_regulator()
Call Mixer()
Call Send_data()


Loop


Measure:
If Channel > 0 And Channel < 5 Then
Kanal(channel) = Timer1
End If
Timer1 = 1536
Incr Channel
Return

Pausedetect:
Channel = 0
Return


Sub Wmp_init()
I2cstart
I2cwbyte &HA6 ' sends memory address
I2cwbyte &HFE ' WM+ activation
I2cwbyte &H04 . ' Now Adress changes to &HA4
I2cstop
End Sub

Sub Send_zero()
I2cstart
I2cwbyte &HA4 ' sends memory address
I2cwbyte &H00 ' sends zero before receiving
I2cstop
Waitms 1
End Sub

Sub Read_data()
Gosub Send_zero ' sends zero before receiving
I2creceive &HA4 , Buffer(1) , 0 , 6 ' receive 6 bytes
Yaw1 = Buffer(1)
Roll1 = Buffer(2) ' Low Bytes
Pitch1 = Buffer(3)
Shift Buffer(4) , Right , 2 : Yaw0 = Buffer(4)
Shift Buffer(5) , Right , 2 : Roll0 = Buffer(5) ' High Bytes
Shift Buffer(6) , Right , 2 : Pitch0 = Buffer(6)
End Sub

Sub Set_offset()
_yawoffset = 0
_rolloffset = 0
_pitchoffset = 0
For I = 1 To 100
Call Read_data
_yawoffset = _yawoffset + Yaw
_rolloffset = _rolloffset + Roll
_pitchoffset = _pitchoffset + Pitch
Next I
_yawoffset = _yawoffset / 100
_rolloffset = _rolloffset / 100
_pitchoffset = _pitchoffset / 100
End Sub



Sub Filter_gyro_data()
Call Read_data()
_yawnow = Yaw - _yawoffset
_rollnow = Roll - _rolloffset
_pitchnow = Pitch - _pitchoffset

_yawnow = _yawnow / 2
_rollnow = _rollnow / 2
_pitchnow = _pitchnow / 2

End Sub



Sub Pid_regulator()
_yaw_err = _sbl(4) / 1
_yaw_err = _yaw_err - _yawnow

_roll_err = _sbl(3) / 5
_roll_err = _roll_err - _rollnow

_pitch_err = _sbl(2) / 5
_pitch_err = _pitch_err - _pitchnow


_yaw_kp_err = _yaw_kp_err * _yaw_kp
_yaw_kp_err = _yaw_err / 10

_roll_kp_err = _roll_kp_err * _roll_kp
_roll_kp_err = _roll_err / 10

_pitch_kp_err = _pitch_kp_err * _pitch_kp
_pitch_kp_err = _pitch_err / 10


_yaw_ki_err = _yaw_ki_err * _yaw_ki
_yaw_ki_err = _yaw_err / 50
_yaw_ki_sum = _yaw_ki_sum + _yaw_ki_err

_roll_ki_err = _roll_ki_err * _roll_ki
_roll_ki_err = _roll_err / 50
_roll_ki_sum = _roll_ki_sum + _roll_ki_err

_pitch_ki_err = _pitch_ki_err * _pitch_ki
_pitch_ki_err = _pitch_err / 50
_pitch_ki_sum = _pitch_ki_sum + _pitch_ki_err


_yaw_kd_err = _yaw_kd_err * _yaw_kd
_yaw_kd_err = _yaw_err / 50
_yaw_kd_err = _yaw_kd_old - _yaw_kd_err
_yaw_kd_old = _yaw_kd_old

_roll_kd_err = _roll_kd_err * _roll_kd
_roll_kd_err = _roll_err / 50
_roll_kd_err = _roll_kd_old - _roll_kd_err
_roll_kd_old = _roll_kd_old

_pitch_kd_err = _pitch_kd_err * _pitch_kd
_pitch_kd_err = _pitch_err / 50
_pitch_kd_err = _pitch_kd_old - _pitch_kd_err
_pitch_kd_old = _pitch_kd_old


_yaw_pid = _yaw_kp_err + _yaw_ki_sum
_yaw_pid = _yaw_pid + _yaw_kd_err

_roll_pid = _roll_kp_err + _roll_ki_sum
_roll_pid = _roll_pid + _roll_kd_err

_pitch_pid = _pitch_kp_err + _pitch_ki_sum
_pitch_pid = _pitch_pid + _pitch_kd_err


If _yaw_pid < -1000 Then _yaw_pid = -1000
If _yaw_pid > 1000 Then _yaw_pid = 1000
If _roll_pid < -1000 Then _roll_pid = -1000
If _roll_pid > 1000 Then _roll_pid = 1000
If _pitch_pid < -1000 Then _pitch_pid = -1000
If _pitch_pid > 1000 Then _pitch_pid = 1000
End Sub


Sub Filter_rx_data()

For I = 1 To 4
_sbl(i) = Kanal(i) - _empfmiddle(i)
_sbl(i) = _sbl(i) / _empfdiv(i)
Next I

'_sbl(4) = _sbl(4) * -1

_sbl(2) = _sbl(2) / 2
_sbl(3) = _sbl(3) / 2
'_sbl(4) = _sbl(4) / 2

'IIR-Filter
'filter_wert = filter_wert * s + wert * (1.0-s)

For I = 1 To 4
_sbl_filter(i) = _sbl_filter(i) * 6
_sbl_filter(i) = _sbl_filter(i) / 10
_sbl_filter_2(i) = _sbl(i) * 4
_sbl_filter_2(i) = _sbl_filter_2(i) / 10
_sbl_filter(i) = _sbl_filter(i) + _sbl_filter_2(i)
_sbl(i) = _sbl_filter(i)
Next I

End Sub


Sub Mixer()
_bl(1) = 62667 - _sbl(1)
_bl(2) = _bl(1)
_bl(3) = _bl(1)
_bl(4) = 62667

_bl(1) = _bl(1) + _bl1offset
_bl(2) = _bl(2) + _bl2offset
_bl(3) = _bl(3) + _bl3offset
_bl(4) = _bl(4) + _bl4offset

_bl(1) = _bl(1) + _pitch_pid
_bl(2) = _bl(2) + _roll_pid
_bl(3) = _bl(3) - _roll_pid
_bl(4) = _bl(4) - _yaw_pid
End Sub


Sub Send_data()
If Channel <= 11 Then
_crc = Crc16(_bl(1) , 4)
Printbin Start_byte ; _bl(1) ; _bl(2) ; _bl(3) ; _bl(4) ; _crc
End If
End Sub


End

Gruß
Chris

EDIT:
Weiß das keiner? Evtl. sollte ich meine Frage nochmal umformulieren:
Welchen Filter würdet ihr persönlich nehmen, um die Gyro-Daten vom Wii Motion Plus Gyro zu filtern?

[lokirobotics]

Du müsstest mal genau erklären, was du eigentlich filtern möchtest.
Die von dir angesprochenen Filter dienen ja dazu Signale bestimmter Frequenzen passieren zu lassen, oder sie halt zu blocken.
Den Drift deiner Gyros wirst du mit einem Frequenzfilter aber nicht kompensiert bekommen.
Wenn das Gyro "driftet" heißt das ja, dass es eine Drehrate ausgibt, die von der tatsächlichen Drehrate abweicht.
Beispielsweise gibt das Gyro immer eine Drehrate aus, die um den Faktor 0,1 größer ist, als die Tatsächliche.
Wenn du über diesen Wert integrierst, wird die Abweichung von deiner Systemdarstellung zum Systemzustand mit der Zeit immer größer.

Du musst den Drift also Kompensieren, nicht filtern.
Ich würde dazu ersteinmal das Driftverhalten der Gyros analysieren (stetiger Drift, variabler Drift) und darauf dann entsprechende Gegenmaßnahmen einleiten.

[Che Guevara]

Hallo,

also einerseits möchte ich das RC-Empfänger Signal filtern, d.h. Störungen rausholen. Diese Störungen sind ja eher hochfrequenter Art, also brauche ich einen Tiefpass (IIR-Filter).. Sehe ich das so richtig?
Dann möchte ich noch den Drift vom Gyro kompensieren, aber ich weiß eben nicht, wie ich das machen soll!? ... Das mit dem Bestimmen des Drifts werde ich demnächst mal machen! Aber was dann, wenn ich weiß, dass der Drift z.b. variabel ist? Dann ist meine einzige Rettung ein ACC oder?

Gruß
Chris

[lokirobotics]

Zu den Störungen kann ich nichts sagen, da ich den Sensor nicht kenne. Klingt aber erstmal plausibel.

Variabler Drift wäre natürlich ganz schlecht. Wenn er aber konstant ist, oder sich als Funktion der Zeit und evtl. der Temperatur darstellen lässt, müsste man ganz gut kompensieren können.
Ggf. könnte man auch die Änderung der Drehrate als "Frequenz" auffassen und die Änderungen durch einen Hochpass schicken.

Ich hab mich noch nicht mit Driftkompensation auseinandergesetzt. Das sind jetzt so die Sachen, die mir dazu einfallen.

Lg

[PICture]

Hallo!

Eigentlich als zufälliges Signall, lässt sich Drift praktisch nicht kompensieren, höchstens minimieren. ;)

[Che Guevara]

Hallo,

das mit der Nichtkompensierbarkeit *gg* von dem variablen Drift habe ich mir schon fast gedacht, deswegen auch meine Frage...
Heute hab ich mir mal einen NUNCHUK zugelegt; dieser ist bereits zerlegt und demnächst werde ich mal eine Platine ätzen, auf der sich der Nunchuk und die WM+ befinden... Diese wird dann testweise am PC angesteckt (über ein RN-Control), um zu sehen, wie ich die Werte am besten interpretiere / filtere / etc...
Wenn ich neue Infos habe, melde ich mich mal wieder, wird so gegen 9 / 10 heute Abend sein (wenn ichs noch schaffe).

Gruß
Chris

[Che Guevara]

So, ist ein bisschen später geworden, aber die Platine ist fertig und bestückt! Auf ihr befinden sich ein LM317 (SMD), 4 Wiederstande, 2 Kondensatoren, eine Wii Motion Plus und eine Nunchuk.
Ich kann beide Sensoren direkt hintereinander mittels i2c auslesen; beim WM+ bekomme ich akzeptable Werte (ich shifte die tatsächlichen Werte um 2 nach rechts, sprich ich teile durch 4), somit ist kein Drift mehr vorhanden (bzw. er ist normalweise schon so klein, dass er jetzt wegfällt). Hab zwar schon öfter gelesen, dass die WM+ ziemlich viel Drift hat, das kann ich jedoch nicht(!) bestätigen!
Leider verstehe ich den Nunchuk nicht so ganz... Einer der Werte (acc-z) steht für mich in keinem Verhältnis zu irgendeiner Bewegung der Platine?! ... Der Wert verändert sich zwar manchmal, aber ich wei0 nicht, wie genau ich die Platine bewegen muss, damit das passiert? Wo ist beim Nunchuk die Z-Achse?

Nun aber zu meiner eigentlichen Frage:
Wie kann ich die ACC-Daten mit den Gyros verrechnen? Irgendwo hab ich in letzer Zeit mal gelesen, dass man mit einem Kalman-Filter diese beiden Senoren kombinieren kann?! Hier im Forum gibt es ja bereits einen BASCOM-Code für einen Kalman, jedoch wei0 ich nicht, wo ich da die ACC-Werte einsetzen soll. Wäre toll, wenn mir das jemand erklären könnte?!
Hier mal mein momentaner Code:


$regfile = "m32def.dat"
$crystal = 16000000
$framesize = 150
$hwstack = 150
$swstack = 150


Declare Sub Pid_regulator()

Declare Sub Wmp_init()
Declare Sub Send_zero()
Declare Sub Read_wmp_data()
Declare Sub Set_wmp_offset()

Declare Sub Nunchuk_init()
Declare Sub Nunchuk_read()
Declare Sub Set_nunchuk_offset()

Declare Function Tastenabfrage() As Byte



Config Adc = Single , Prescaler = Auto
Start Adc

Config Pina.7 = Input
Porta.7 = 1


Dim Taste As Byte
Dim _direction As Byte
_direction = 1


$lib "I2C_TWI.LBX"
Config Scl = Portc.0
Config Sda = Portc.1
Config Twi = 100000
I2cinit


Config Lcd = 16 * 2
Config Lcdpin = Pin , Db4 = Portb.0 , Db5 = Portb.1 , Db6 = Portb.2 , Db7 = Portb.3 , E = Portb.4 , Rs = Portb.5
Config Lcdbus = 4
Initlcd
Cursor Off
Cls

Locate 1 , 1
Lcd "*WM+ // NUNCHUK*"


Dim Nunchuk_buffer(6) As Byte
Dim Wmplus_buffer(6) As Byte


Dim Yaw As Word
Dim Yaw0 As Byte At Yaw + 1 Overlay
Dim Yaw1 As Byte At Yaw Overlay

Dim Roll As Word
Dim Roll0 As Byte At Roll + 1 Overlay
Dim Roll1 As Byte At Roll Overlay

Dim Pitch As Word
Dim Pitch0 As Byte At Pitch + 1 Overlay
Dim Pitch1 As Byte At Pitch Overlay

Dim _yawoffset As Long
Dim _rolloffset As Long
Dim _pitchoffset As Long

Dim _yawoffset_int As Integer
Dim _rolloffset_int As Integer
Dim _pitchoffset_int As Integer

Dim _yawnow As Integer
Dim _rollnow As Integer
Dim _pitchnow As Integer


Dim Acc_x As Byte
Dim Acc_y As Byte
Dim Acc_z As Byte

Dim Acc_x_now As Integer
Dim Acc_y_now As Integer
Dim Acc_z_now As Integer

Dim Acc_x_offset As Word
Dim Acc_y_offset As Word
Dim Acc_z_offset As Word

Dim Acc_x_offset_int As Integer
Dim Acc_y_offset_int As Integer
Dim Acc_z_offset_int As Integer

Dim Tmp As Byte
Dim I As Byte


Dim _yaw_kp_err As Integer
Dim _roll_kp_err As Integer
Dim _pitch_kp_err As Integer

Dim _yaw_ki_err As Integer
Dim _roll_ki_err As Integer
Dim _pitch_ki_err As Integer

Dim _yaw_ki_sum As Integer
Dim _roll_ki_sum As Integer
Dim _pitch_ki_sum As Integer

Dim _yaw_kd_err As Integer
Dim _roll_kd_err As Integer
Dim _pitch_kd_err As Integer

Dim _yaw_kd_old As Integer
Dim _roll_kd_old As Integer
Dim _pitch_kd_old As Integer

Dim _yaw_pid As Integer
Dim _roll_pid As Integer
Dim _pitch_pid As Integer

Dim _yaw_err As Integer
Dim _roll_err As Integer
Dim _pitch_err As Integer



Dim _sbl(4) As Integer
_sbl(1) = 0
_sbl(2) = 0
_sbl(3) = 0
_sbl(4) = 0


Const _yaw_kp = 1000
Const _roll_kp = 1000
Const _pitch_kp = 1000

Const _yaw_ki = 100
Const _roll_ki = 100
Const _pitch_ki = 100

Const _yaw_kd = 100
Const _roll_kd = 100
Const _pitch_kd = 100


Dim _yaw_filter As Integer
Dim _yaw_filter_2 As Integer
Dim _roll_filter As Integer
Dim _roll_filter_2 As Integer
Dim _pitch_filter As Integer
Dim _pitch_filter_2 As Integer


'DEBUG!!!
Dim Min_yaw As Integer
Dim Max_yaw As Integer
Dim Min_roll As Integer
Dim Max_roll As Integer
Dim Min_pitch As Integer
Dim Max_pitch As Integer
'########



Call Nunchuk_init()
Call Wmp_init()
Call Set_wmp_offset()
Call Set_nunchuk_offset()
Cls



Do


Call Read_wmp_data()
Call Nunchuk_read()

_yawnow = Yaw - _yawoffset_int
_rollnow = Roll - _rolloffset_int
_pitchnow = Pitch - _pitchoffset_int

Acc_x_now = Acc_x - Acc_x_offset_int
Acc_y_now = Acc_y - Acc_y_offset_int
Acc_z_now = Acc_z - Acc_z_offset_int

Call Pid_regulator()

If _yaw_pid < Min_yaw Then Min_yaw = _yaw_pid
If _yaw_pid > Max_yaw Then Max_yaw = _yaw_pid
If _roll_pid < Min_roll Then Min_roll = _roll_pid
If _roll_pid > Max_roll Then Max_roll = _roll_pid
If _pitch_pid < Min_pitch Then Min_pitch = _pitch_pid
If _pitch_pid > Max_pitch Then Max_pitch = _pitch_pid

Taste = Tastenabfrage()
If Taste <> 0 Then
If Taste = 1 Then Decr _direction
If Taste = 2 Then Incr _direction
If _direction > 3 Then _direction = 1
If _direction < 1 Then _direction = 3
End If

Cls
Locate 1 , 1
Lcd _yaw_pid ; " : " ; _roll_pid ; " : " ; _pitch_pid ; " "

Locate 2 , 1
If _direction = 1 Then
Lcd "YAW: " ; Min_yaw ; "/" ; Max_yaw ; " "
Elseif _direction = 2 Then
Lcd "ROLL: " ; Min_roll ; "/" ; Max_roll ; " "
Elseif _direction = 3 Then
Lcd "PITCH: " ; Min_pitch ; "/" ; Max_pitch ; " "
End If


'(
Cls
Locate 1 , 1
Lcd _yawnow
Locate 1 , 6
Lcd ": " ; _rollnow
Locate 1 , 11
Lcd ": " ; _pitchnow ; " "
Locate 2 , 1
Lcd Acc_x_now
Locate 2 , 6
Lcd ": " ; Acc_y_now
Locate 2 , 11
Lcd ": " ; Acc_z_now
Waitms 100
')

Loop


Sub Nunchuk_init()
I2cstart
I2cwbyte &HA4
I2cwbyte &H40
I2cwbyte &H00 .
I2cstop
End Sub

Sub Nunchuk_read()
I2cstart
I2cwbyte &HA4
I2cwbyte &H00
I2cstop
Waitms 1
Nunchuk_buffer(1) = 0
I2creceive &HA5 , Nunchuk_buffer(1) , 0 , 6
Acc_x = Nunchuk_buffer(3) Eor &H17
Acc_x = Acc_x + &H17
Acc_y = Nunchuk_buffer(4) Eor &H17
Acc_y = Acc_y + &H17
Acc_z = Nunchuk_buffer(5) Eor &H17
Acc_z = Acc_z + &H17
End Sub

Sub Set_nunchuk_offset()
Acc_x_offset = 0
Acc_y_offset = 0
Acc_z_offset = 0
For I = 1 To 100
Call Nunchuk_read()
Acc_x_offset = Acc_x_offset + Acc_x
Acc_y_offset = Acc_y_offset + Acc_y
Acc_z_offset = Acc_z_offset + Acc_z
Next I
Acc_x_offset = Acc_x_offset / 100
Acc_y_offset = Acc_y_offset / 100
Acc_z_offset = Acc_z_offset / 100
Acc_x_offset_int = Acc_x_offset
Acc_y_offset_int = Acc_y_offset
Acc_z_offset_int = Acc_z_offset
End Sub


Sub Wmp_init()
I2cstart
I2cwbyte &HA6
I2cwbyte &HFE
I2cwbyte &H04
I2cstop
End Sub

Sub Send_zero()
I2cstart
I2cwbyte &HA4
I2cwbyte &H00
I2cstop
End Sub

Sub Read_wmp_data()
Gosub Send_zero
I2creceive &HA4 , Wmplus_buffer(1) , 0 , 6
Yaw1 = Wmplus_buffer(1)
Roll1 = Wmplus_buffer(2)
Pitch1 = Wmplus_buffer(3)
Shift Wmplus_buffer(4) , Right , 2 : Yaw0 = Wmplus_buffer(4)
Shift Wmplus_buffer(5) , Right , 2 : Roll0 = Wmplus_buffer(5)
Shift Wmplus_buffer(6) , Right , 2 : Pitch0 = Wmplus_buffer(6)
Shift Yaw , Right , 2
Shift Roll , Right , 2
Shift Pitch , Right , 2

'IIR-Filter
'filter_wert = filter_wert * s + wert * (1.0-s)

_yaw_filter = _yaw_filter * 4
_yaw_filter = _yaw_filter / 10
_yaw_filter_2 = Yaw * 6
_yaw_filter_2 = _yaw_filter_2 / 10
_yaw_filter = _yaw_filter + _yaw_filter_2
Yaw = _yaw_filter

_roll_filter = _roll_filter * 4
_roll_filter = _roll_filter / 10
_roll_filter_2 = Roll * 6
_roll_filter_2 = _roll_filter_2 / 10
_roll_filter = _roll_filter + _roll_filter_2
Roll = _roll_filter

_pitch_filter = _pitch_filter * 4
_pitch_filter = _pitch_filter / 10
_pitch_filter_2 = Pitch * 6
_pitch_filter_2 = _pitch_filter_2 / 10
_pitch_filter = _pitch_filter + _pitch_filter_2
Pitch = _pitch_filter

End Sub

Sub Set_wmp_offset()
_yawoffset = 0
_rolloffset = 0
_pitchoffset = 0
For I = 1 To 100
Call Read_wmp_data()
_yawoffset = _yawoffset + Yaw
_rolloffset = _rolloffset + Roll
_pitchoffset = _pitchoffset + Pitch
Next I
_yawoffset = _yawoffset / 100
_rolloffset = _rolloffset / 100
_pitchoffset = _pitchoffset / 100
_yawoffset_int = _yawoffset + 10
_rolloffset_int = _rolloffset + 10
_pitchoffset_int = _pitchoffset + 10
End Sub

Sub Pid_regulator()
_yaw_err = _sbl(4) / 1
_yaw_err = _yaw_err - _yawnow

_roll_err = _sbl(3) / 5
_roll_err = _roll_err - _rollnow

_pitch_err = _sbl(2) / 5
_pitch_err = _pitch_err - _pitchnow


_yaw_kp_err = _yaw_kp_err * _yaw_kp
_yaw_kp_err = _yaw_err / 10

_roll_kp_err = _roll_kp_err * _roll_kp
_roll_kp_err = _roll_err / 10

_pitch_kp_err = _pitch_kp_err * _pitch_kp
_pitch_kp_err = _pitch_err / 10


_yaw_ki_err = _yaw_ki_err * _yaw_ki
_yaw_ki_err = _yaw_err / 50
_yaw_ki_sum = _yaw_ki_sum + _yaw_ki_err

_roll_ki_err = _roll_ki_err * _roll_ki
_roll_ki_err = _roll_err / 50
_roll_ki_sum = _roll_ki_sum + _roll_ki_err

_pitch_ki_err = _pitch_ki_err * _pitch_ki
_pitch_ki_err = _pitch_err / 50
_pitch_ki_sum = _pitch_ki_sum + _pitch_ki_err


_yaw_kd_err = _yaw_kd_err * _yaw_kd
_yaw_kd_err = _yaw_err / 50
_yaw_kd_err = _yaw_kd_old - _yaw_kd_err
_yaw_kd_old = _yaw_kd_old

_roll_kd_err = _roll_kd_err * _roll_kd
_roll_kd_err = _roll_err / 50
_roll_kd_err = _roll_kd_old - _roll_kd_err
_roll_kd_old = _roll_kd_old

_pitch_kd_err = _pitch_kd_err * _pitch_kd
_pitch_kd_err = _pitch_err / 50
_pitch_kd_err = _pitch_kd_old - _pitch_kd_err
_pitch_kd_old = _pitch_kd_old


_yaw_pid = _yaw_kp_err + _yaw_ki_sum
_yaw_pid = _yaw_pid + _yaw_kd_err

_roll_pid = _roll_kp_err + _roll_ki_sum
_roll_pid = _roll_pid + _roll_kd_err

_pitch_pid = _pitch_kp_err + _pitch_ki_sum
_pitch_pid = _pitch_pid + _pitch_kd_err


If _yaw_pid < -1000 Then _yaw_pid = -1000
If _yaw_pid > 1000 Then _yaw_pid = 1000
If _roll_pid < -1000 Then _roll_pid = -1000
If _roll_pid > 1000 Then _roll_pid = 1000
If _pitch_pid < -1000 Then _pitch_pid = -1000
If _pitch_pid > 1000 Then _pitch_pid = 1000
End Sub

Function Tastenabfrage() As Byte
Local Ws As Word
Tastenabfrage = 0
Start Adc
Ws = Getadc(7)
If Ws < 500 Then
Select Case Ws
Case 400 To 450
Tastenabfrage = 1
Case 330 To 380
Tastenabfrage = 2
Case 260 To 305
Tastenabfrage = 3
Case 180 To 220
Tastenabfrage = 4
Case 90 To 130
Tastenabfrage = 5
End Select
End If
Waitms 100
End Function

End


Wenn jemand ein Video vom Display sehen möchte, kann ichs schnell filmen! Die WM+ liegt seit ca. 30 Min neben mir und es gab noch keinen einzigen (!!!) "Ausbrecher". Ich denke, das genügt vollkommen zum fliegen?!
ABER:
Durch meine Division durch 4 verliere ich an Auflösung, d.h. ich muss auch die Werte für kp, ki, kd extrem hochschrauben... Denkt ihr, das ist ok oder würdet ihr das anders lösen?

Viele Fragen auf einmal, ich hoffe auf zahlreiche Antworten :)
Gruß
Chris

[lokirobotics]

Guck dir mal das hier (http://tom.pycke.be/mav/92/kalman-demo-application) an. Driftkompensation durch Zusammenarbeit von Gyro und Beschleunigungssensor mit Kalman Filter.

[Che Guevara]

Danke für den Tip, die Seite scheint ganz gut zu sein :) Leider weiß ich nicht, mit welchem Programm ich die Files (und va inwelchem ist der kalman filter) öffnen kann? Geht das mit AVRStudio? Außerdem verstehe ich die C-Syntax nicht... Aber dennoch möchte ichs mir ansehen, wenn mir jemand das richtige Programm nennt.

Gruß
Chris

[lokirobotics]

Der Kalman Filter ist in den Dateien ars.h und ars.c implementiert.
In diesem Artikel (http://tom.pycke.be/mav/71/kalman-filtering-of-imu-data) werden die Grundlagen zum Filter erklärt.

[Che Guevara]

Also ich hab mir jetzt die 2 Datein angesehn (mit AVRStudio) und muss sagen, ich verstehe leider fast garnichts :( Was bedeutet z.b. der "->" (Pfeil) bei


filterdata->xx = ...


Gibts nicht evtl. einen anderen Code, den ich als BASCOM-Jünger besser verstehe? Ich weiß, dass hier im Netz auch ein Kalman-Filter in BASCOM geschrieben wurde, diesen würde ich zumindest soweit verstehen, dass ich Änderungen vornehmen könnte! Leider weiß ich aber nicht, wie ich da den ACC mit einrechnen soll, also mittels welcher Variable?

Gruß
Chris

[lokirobotics]

filterdata ist eine Variable der Datenstruktur ...1DKalmanFilter... "->" ist der Zugriffsoperator, mit dem auf die Felder in der Datenstruktur zugegriffen werden kann.

[Che Guevara]

Hm, ich glaube, der Kalman-Filter ist mir zu kompliziert! Aber Willa betont ja auch immer, ein Komplementärfilter ist genauso gut wie ein Kalman :D Gibts da evtl. ein paar mehr Beispiele für so einen Komplementärfilter? Bis jetzt habe ich nur 1 Beispiel im web gefunden, welches jedoch für mich sehr unverständlich ist, da nicht viel kommentiert wurde... Hat da jemand was?

Gruß
Chris