Merge pull request #40 from Rione:feature/kalman-params-flag

🎉 add kalman filter parameter flags
Rione · Jul 6, 2024 · 97118b9 · 97118b9
2 parents bdafbbb + c386e18
commit 97118b9
Show file tree

Hide file tree

Showing 2 changed files with 28 additions and 25 deletions.
diff --git a/main.go b/main.go
@@ -211,23 +211,26 @@ func RunServer(chserver chan bool, reportrate uint, ourteam int, goalpose int, d
 func main() {
 
 	var (
-		visionport          = flag.Int("p", 10006, "Vision Multicast Port Number. Force 10006 when match mode is true")
-		gcport              = flag.Int("gcp", 10003, "Game Controller Listen Port Number (Default 10003) Force 10003 when match mode is true")
-		ourteam             = flag.String("t", "blue", "Our Team (blue or yellow). Disable when match mode is true")
-		goalpos             = flag.String("g", "N", "Attack Direction(Enemy goal) Negative or Positive (N or P)")
-		reportrate          = flag.Uint("r", 16, "How often report to RACOON-AI? (milliseconds)")
-		debug               = flag.Bool("d", false, "Show All Send Packet")
-		simmode             = flag.Bool("s", false, "Simulation Mode (Emulate Ball Sensor). Disable when match mode is true")
-		replay              = flag.Bool("replay", false, "Replay All Packet")
-		halfswitch          = flag.String("c", "F", "Where to use (N, P, F) F to Full. Disable when match mode is true")
-		ballmovethreshold   = flag.Float64("b", 1000, "Ball Detect Threshold (Default 1000")
-		nw_robot            = flag.String("rif", "none", "NW Robot Update Interface Name (ex. en0)")
-		nw_vision           = flag.String("vif", "none", "NW Vision and Referee receive Interface Name (ex. en1)")
-		debug_for_sono      = flag.Bool("df", false, "Print ID0 Robot Cordination for Sono")
-		ignore_ref_mismatch = flag.Bool("igref", false, "Ignore Referee Team Color & Attack Direction Mismatch Errors. Disable when match mode is true")
-		match_mode          = flag.Bool("m", false, "Match Mode (Disable Some Options! Most option get from GC)")
-		grsim_send_port     = flag.Int("grsimport", 20011, "grSim Command Listen Port Number")
-		goal_keeper         = flag.Uint("gk", 0, "Goal Keeper ID (0-15) Disable when match mode is true")
+		visionport           = flag.Int("p", 10006, "Vision Multicast Port Number. Force 10006 when match mode is true")
+		gcport               = flag.Int("gcp", 10003, "Game Controller Listen Port Number (Default 10003) Force 10003 when match mode is true")
+		ourteam              = flag.String("t", "blue", "Our Team (blue or yellow). Disable when match mode is true")
+		goalpos              = flag.String("g", "N", "Attack Direction(Enemy goal) Negative or Positive (N or P)")
+		reportrate           = flag.Uint("r", 16, "How often report to RACOON-AI? (milliseconds)")
+		debug                = flag.Bool("d", false, "Show All Send Packet")
+		simmode              = flag.Bool("s", false, "Simulation Mode (Emulate Ball Sensor). Disable when match mode is true")
+		replay               = flag.Bool("replay", false, "Replay All Packet")
+		halfswitch           = flag.String("c", "F", "Where to use (N, P, F) F to Full. Disable when match mode is true")
+		ballmovethreshold    = flag.Float64("b", 1000, "Ball Detect Threshold (Default 1000")
+		nw_robot             = flag.String("rif", "none", "NW Robot Update Interface Name (ex. en0)")
+		nw_vision            = flag.String("vif", "none", "NW Vision and Referee receive Interface Name (ex. en1)")
+		debug_for_sono       = flag.Bool("df", false, "Print ID0 Robot Cordination for Sono")
+		ignore_ref_mismatch  = flag.Bool("igref", false, "Ignore Referee Team Color & Attack Direction Mismatch Errors. Disable when match mode is true")
+		match_mode           = flag.Bool("m", false, "Match Mode (Disable Some Options! Most option get from GC)")
+		grsim_send_port      = flag.Int("grsimport", 20011, "grSim Command Listen Port Number")
+		goal_keeper          = flag.Uint("gk", 0, "Goal Keeper ID (0-15) Disable when match mode is true")
+		initial_variance     = flag.Float64("iv", 100, "Initial Variance for Ball Kalman Filter(Default 100)")
+		process_variance     = flag.Float64("pv", 0.1, "Process Variance for Ball Kalman Filter(Default 0.1)")
+		observation_variance = flag.Float64("ov", 0.18, "Observation Variance for Ball Kalman Filter(Default 0.18)")
 	)
 	//OUR TEAM 0 = blue
 	//OUR TEAM 1 = yellow
@@ -296,7 +299,7 @@ func main() {
 
 	go Update(chupdate)
 	go RunServer(chserver, *reportrate, ourteam_n, goalpos_n, *debug, *simmode, *ignore_ref_mismatch, *match_mode, *grsim_send_port, *goal_keeper, halfswitch_n)
-	go VisionReceive(chvision, *visionport, ourteam_n, goalpos_n, *simmode, *replay, halfswitch_n, *debug_for_sono, *match_mode)
+	go VisionReceive(chvision, *visionport, ourteam_n, goalpos_n, *simmode, *replay, halfswitch_n, *debug_for_sono, *match_mode, *initial_variance, *process_variance, *observation_variance)
 	go CheckVisionRobot(chvisrobot)
 	go FPSCounter(chfps, ourteam_n)
 	go RefereeClient(chref, *gcport)

diff --git a/visionreceive.go b/visionreceive.go
@@ -19,7 +19,7 @@ import (
 	"google.golang.org/protobuf/proto"
 )
 
-func VisionReceive(chvision chan bool, port int, ourteam int, goalpos int, simmode bool, replay bool, halfswitch_n int, debug_for_sono bool, matchmode bool) {
+func VisionReceive(chvision chan bool, port int, ourteam int, goalpos int, simmode bool, replay bool, halfswitch_n int, debug_for_sono bool, matchmode bool, initial_variance float64, process_variance float64, observation_variance float64) {
 
 	var pre_ball_X float32
 	var pre_ball_Y float32
@@ -60,18 +60,18 @@ func VisionReceive(chvision chan bool, port int, ourteam int, goalpos int, simmo
 	var enemy_u_k_1 [16]*mat.Dense
 
 	modelBallX = models.NewSimpleModel(t, 0.0, models.SimpleModelConfig{
-		InitialVariance:     100,
-		ProcessVariance:     0.1,
-		ObservationVariance: 0.18,
+		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:     100,
-		ProcessVariance:     0.1,
-		ObservationVariance: 0.08,
+		InitialVariance:     initial_variance,
+		ProcessVariance:     process_variance,
+		ObservationVariance: observation_variance,
 	})
 	filterBallY := kalman.NewKalmanFilter(modelBallY)
 	// smoothedBallY := kalman.NewKalmanSmoother(modelBallX)