Skip to content

Commit

Permalink
Merge pull request #43 from Rione/ball_kalmanfilter
Browse files Browse the repository at this point in the history 
Ball kalmanfilter
  • Loading branch information
@Tamagoham
Tamagoham authored Jul 9, 2024
2 parents 8e5e373 + decd8f3 commit ac6db92
Showing 1 changed file with 118 additions and 54 deletions.
172 changes: 118 additions & 54 deletions visionreceive.go
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,8 +13,8 @@ import (
"time"

"github.com/Rione-SSL/RACOON-MW/proto/pb_gen"
"github.com/rosshemsley/kalman"
"github.com/rosshemsley/kalman/models"
// "github.com/rosshemsley/kalman"
// "github.com/rosshemsley/kalman/models"
"gonum.org/v1/gonum/mat"
"google.golang.org/protobuf/proto"
)
Expand All @@ -31,10 +31,10 @@ func VisionReceive(chvision chan bool, port int, ourteam int, goalpos int, simmo
var pre_enemy_Y [16]float32
var pre_enemy_Theta [16]float32

var t time.Time
// var t time.Time

var modelBallX *models.SimpleModel
var modelBallY *models.SimpleModel
// var modelBallX *models.SimpleModel
// var modelBallY *models.SimpleModel

// var sum float64 = 0
// var sumx float64 = 0
Expand All @@ -59,21 +59,45 @@ func VisionReceive(chvision chan bool, port int, ourteam int, goalpos int, simmo
var enemy_P_k_1 [16]*mat.Dense
var enemy_u_k_1 [16]*mat.Dense

modelBallX = models.NewSimpleModel(t, 0.0, models.SimpleModelConfig{
InitialVariance: initial_variance,
ProcessVariance: process_variance,
ObservationVariance: observation_variance,
})
filterBallX := kalman.NewKalmanFilter(modelBallX)
//KalmanSmoother
// smoothedBallX := kalman.NewKalmanSmoother(modelBallX)

modelBallY = models.NewSimpleModel(t, 0.0, models.SimpleModelConfig{
InitialVariance: initial_variance,
ProcessVariance: process_variance,
ObservationVariance: observation_variance,
})
filterBallY := kalman.NewKalmanFilter(modelBallY)
var m float64 = 0.046 //[kg] ボールの質量
var mu float64 = 0.05 //[N・s/m] ボールの粘性摩擦抵抗
var Ts float64 = 0.016 //[s] サンプリング周期
var K float64 = 20.0
var Ad_lowpass float64 = 0.818731
var Bd_lowpass float64 = 0.181269
var DeltaPmax float64 = 6 * Ts * 2
var ObPosX_k_1 float64 = 0.0
var ObPosY_k_1 float64 = 0.0
var ObPosX_lowpass float64 = 0.0
var ObPosY_lowpass float64 = 0.0
var ObPosX_lowpass_k_1 float64 = 0.0
var ObPosY_lowpass_k_1 float64 = 0.0
var ObVelX float64 = 0.0
var ObVelY float64 = 0.0
var ObVelX_k_1 float64 = 0.0
var ObVelY_k_1 float64 = 0.0
var tempX_1 float64 = 0.0
var tempY_1 float64 = 0.0
var tempX_2 float64 = 0.0
var tempY_2 float64 = 0.0
var ObPosX float64 = 0.0
var ObPosY float64 = 0.0

// modelBallX = models.NewSimpleModel(t, 0.0, models.SimpleModelConfig{
// InitialVariance: initial_variance,
// ProcessVariance: process_variance,
// ObservationVariance: observation_variance,
// })
// filterBallX := kalman.NewKalmanFilter(modelBallX)
// //KalmanSmoother
// // smoothedBallX := kalman.NewKalmanSmoother(modelBallX)

// modelBallY = models.NewSimpleModel(t, 0.0, models.SimpleModelConfig{
// InitialVariance: initial_variance,
// ProcessVariance: process_variance,
// ObservationVariance: observation_variance,
// })
// filterBallY := kalman.NewKalmanFilter(modelBallY)
// smoothedBallY := kalman.NewKalmanSmoother(modelBallX)

// var modelRobotX [16]*models.SimpleModel
Expand Down Expand Up @@ -215,6 +239,9 @@ func VisionReceive(chvision chan bool, port int, ourteam int, goalpos int, simmo
ourteam = teamcolor_from_ref
}

var is_ball_exists bool = false
var flag_ball bool = false
// var get_ball bool = false
for i := 0; i < maxcameras; i++ {
var n int
var err error
Expand Down Expand Up @@ -361,7 +388,8 @@ func VisionReceive(chvision chan bool, port int, ourteam int, goalpos int, simmo
PixelX: proto.Float32(0.0),
PixelY: proto.Float32(0.0),
}
var is_ball_exists bool = false
// is_ball_exists = true

if packet.Detection.GetBalls() != nil {
var usethisball bool
for _, fball := range packet.Detection.GetBalls() {
Expand All @@ -383,12 +411,18 @@ func VisionReceive(chvision chan bool, port int, ourteam int, goalpos int, simmo
maxconfball = fball
}
is_ball_exists = true
flag_ball = true
}
}

if is_ball_exists {
ball = maxconfball
}

if !flag_ball {
flag_ball = true
}

}

}
Expand Down Expand Up @@ -419,44 +453,74 @@ func VisionReceive(chvision chan bool, port int, ourteam int, goalpos int, simmo
//
/////////////////////////////////////
if ball != nil {
//if ball_x_history is too long, remove first element
if len(ball_x_history) > 5 {
ball_x_history = ball_x_history[1:]
}
//append ball x history
ball_x_history = append(ball_x_history, ball.GetX())

t = t.Add(time.Duration(secperframe * 1000 * float32(time.Millisecond)))
err := filterBallX.Update(t, modelBallX.NewMeasurement(float64(ball.GetX())))
if err != nil {
log.Println(err)
}
mm_x := make([]*kalman.MeasurementAtTime, len(ball_x_history))
for i, v := range ball_x_history {
mm_x[i] = kalman.NewMeasurementAtTime(t, modelBallX.NewMeasurement(float64(v)))
if framecounter == 1 {
ObPosX_k_1 = float64(ball.GetX() / 1000)
ObPosY_k_1 = float64(ball.GetY() / 1000)
ObPosX_lowpass_k_1 = float64(ball.GetX() / 1000)
ObPosY_lowpass_k_1 = float64(ball.GetY() / 1000)
}

//if ball_x_history is too long, remove first element
if len(ball_y_history) > 5 {
ball_y_history = ball_y_history[1:]
}
//append ball x history
ball_y_history = append(ball_y_history, ball.GetY())
TempX := ObPosX_k_1
TempY := ObPosY_k_1
Temp_lowpassX := 0.0
Temp_lowpassY := 0.0
TempVelX := 0.0
TempVelY := 0.0

ballPosXInMeter := float32(ball.GetX() / 1000)
ballPosYInMeter := float32(ball.GetY() / 1000)

for i := 0; i < 3; i++ {
DeltaX := ballPosXInMeter - float32(ObPosX_lowpass_k_1)
DeltaY := ballPosYInMeter - float32(ObPosY_lowpass_k_1)
if math.Abs(float64(DeltaX)) > DeltaPmax*1000 {
tempX_2 = 0.0
} else {
tempX_2 = float64(ballPosXInMeter) - TempX
}

err = filterBallY.Update(t, modelBallY.NewMeasurement(float64(ball.GetY())))
if err != nil {
log.Println(err)
}
if math.Abs(float64(DeltaY)) > DeltaPmax*1000 {
tempY_2 = 0.0
} else {
tempY_2 = float64(ballPosYInMeter) - TempY
}
if flag_ball {
tempX_1 = tempX_2
tempY_1 = tempY_2
} else {
tempX_1 = 0.0
tempY_1 = 0.0
}

mm_y := make([]*kalman.MeasurementAtTime, len(ball_x_history))
for i, v := range ball_y_history {
mm_y[i] = kalman.NewMeasurementAtTime(t, modelBallY.NewMeasurement(float64(v)))
accX := (K / m) * tempX_1
accY := (K / m) * tempY_1
// log.Println("accX: ", accX, "accY: ", accY)
TempVelX = ObVelX_k_1 + Ts*accX
TempVelY = ObVelY_k_1 + Ts*accY
TempX = ObPosX_k_1 + Ts*TempVelX
TempY = ObPosY_k_1 + Ts*TempVelY
// fmt.Printf("TempX: %f, TempY: %f\n", TempX, TempY)
Temp_lowpassX = Bd_lowpass*TempX + Ad_lowpass*ObPosX_lowpass_k_1
Temp_lowpassY = Bd_lowpass*TempY + Ad_lowpass*ObPosY_lowpass_k_1
}

filtered_ball_x = float32(modelBallX.Value(filterBallX.State()))
filtered_ball_y = float32(modelBallY.Value(filterBallY.State()))
// filtered_ball_x = ball.GetX()
// filtered_ball_y = ball.GetY()
ObPosX = TempX
ObPosY = TempY
ObVelX = TempVelX
ObVelY = TempVelY
ObVelX = ObVelX * math.Exp(-mu*(m*10))
ObVelY = ObVelY * math.Exp(-mu*(m*10))
ObPosX_lowpass = Temp_lowpassX
ObPosY_lowpass = Temp_lowpassY

ObPosX_k_1 = TempX
ObPosY_k_1 = TempY
ObVelX_k_1 = ObVelX
ObVelY_k_1 = ObVelY
ObPosX_lowpass_k_1 = ObPosX_lowpass
ObPosY_lowpass_k_1 = ObPosY_lowpass
filtered_ball_x = float32(ObPosX * 1000)
filtered_ball_y = float32(ObPosY * 1000)

}

Expand All @@ -478,7 +542,7 @@ func VisionReceive(chvision chan bool, port int, ourteam int, goalpos int, simmo
bdY64 := float64(ball_difference_Y)
ball_slope_degree = float32(math.Atan2(bdY64, bdX64))
ball_intercept = ball_Y - (ball_slope * ball_X)
ball_speed = float32(math.Sqrt(math.Pow(bdX64, 2)+math.Pow(bdY64, 2)) / 0.016)
ball_speed = float32(math.Sqrt(math.Pow(bdX64, 2)+math.Pow(bdY64, 2)) / float64(secperframe))
} else {
ball_slope_degree = 0.0
ball_intercept = 0.0
Expand Down

0 comments on commit ac6db92

Please sign in to comment.