From 8cbc68a3be5359b2ed2bf6485077de929a1bea44 Mon Sep 17 00:00:00 2001
From: Shane Smiskol <shane@smiskol.com>
Date: Thu, 24 Aug 2023 14:33:11 -0700
Subject: [PATCH] C++ FirstOrderFilter: add initialized flag (#29602)

add initialized flag with default to not change behavior
old-commit-hash: 54e98fa888c9e28eb93513bada8371a63a54bb55
---
 common/util.h | 11 +++++++++--
 1 file changed, 9 insertions(+), 2 deletions(-)

diff --git a/common/util.h b/common/util.h
index d6ab698245..30f514f8dc 100644
--- a/common/util.h
+++ b/common/util.h
@@ -153,12 +153,18 @@ struct unique_fd {
 
 class FirstOrderFilter {
 public:
-  FirstOrderFilter(float x0, float ts, float dt) {
+  FirstOrderFilter(float x0, float ts, float dt, bool initialized = true) {
     k_ = (dt / ts) / (1.0 + dt / ts);
     x_ = x0;
+    initialized_ = initialized;
   }
   inline float update(float x) {
-    x_ = (1. - k_) * x_ + k_ * x;
+    if (initialized_) {
+      x_ = (1. - k_) * x_ + k_ * x;
+    } else {
+      initialized_ = true;
+      x_ = x;
+    }
     return x_;
   }
   inline void reset(float x) { x_ = x; }
@@ -166,6 +172,7 @@ public:
 
 private:
   float x_, k_;
+  bool initialized_;
 };
 
 template<typename T>