• 새로운 policy_runner_flat.py를 추가했습니다. ONNX(policy: model_flat/exported/policy.onnx)를 불러와 48차원
관측(선속도/각속도/중력/명령/관절상태/직전액션)을 사용하며, PD로 토크를 계산해 MuJoCo/Unitree 채널로 보냅니
다. Kalman 기반 선속도 추정기를 내장해 항상 추정과 성능 로깅을 수행하고, --estimate로 정책 입력에 쓸 속도 소
스(sport vs kalman)를 선택할 수 있습니다. Kalman 필터는 IMU 가속도+자세로 예측하고 SportModeState 선속도를
측정으로 사용해 에러 통계(RMSE/평균)를 주기적으로 출력하며, 이동평균으로 매끄럽게 만듭니다.

주요 사용법:

• 실행: python3 policy_runner_flat.py --estimate kalman --duration 30 --interface lo

  • --estimate sport로 정책 입력에 sport 선속도를 쓰면서도 칼만 추정/로깅은 계속 진행.
  • --disable-noise로 관측 노이즈 비활성화.

• 기본 ONNX 경로: /home/user/unitree_mujoco/example/python/model_flat/exported/policy.onnx

• Kalman 로그는 100스텝마다 policy 입력 속도 / Kalman / sport 속도를 비교해서 터미널에 표시합니다.

  참고: filterpy가 필요합니다(미설치 시 pip install filterpy).

---

• filterpy 의존성을 제거하고 Kalman 필터를 직접 구현했습니다. policy_runner_flat.py는 이제 순수 numpy 기반의
  3D 칼만 필터(SimpleKalman3D)를 사용해 IMU 가속도+자세로 예측하고 SportModeState 속도를 측정으로 융합합니다.
  초기화/스무딩/오차통계 로직은 그대로 유지되어 칼만 추정값과 sport 속도를 항상 비교·로깅합니다. 파이썬 3.8에
  서 추가 패키지 없이 동작합니다.

실행 예:

• python3 policy_runner_flat.py --estimate kalman --duration 30 --interface lo
• 정책 입력 속도 소스만 --estimate로 선택하며, 칼만 추정/성능 로그는 항상 수행됩니다.
• --disable-noise로 관측 노이즈 비활성화.

---

• policy_runner_flat.py 흐름은 다음과 같습니다:

• ONNX 정책 출력(IsaacLab 순서 12개)을 ACTION_SCALE로 스케일 후 DEFAULT_JOINT_POS를 더해 목표 관절 위치
  (IsaacLab 순서)를 만듭니다.
• 목표 위치를 ISAACLAB_TO_UNITREE로 재정렬해 Unitree 순서의 target_positions_unitree를 얻습니다.
• 현재 관절 상태(LowState.motor_state[:12]의 q, dq, Unitree 순서)를 읽어와 PD 토크를 계산합니다: tau =
  KP(q_target - q) + KD(0 - dq), 이후 ACTION_CLIP으로 클리핑.
• build_command에서 unitree_go_msg_dds__LowCmd 메시지를 생성해 0~11번 모터에 계산된 토크를 채우고, CRC를
  계산합니다.
• ChannelPublisher("rt/lowcmd", LowCmd_)로 DDS 채널에 Write 하여 시뮬레이터/브리지로 토크 명령이 전달됩니다.

• policy_runner_flat.py에서 보낸 명령은 다음 경로로 MuJoCo 시뮬레이터에 적용됩니다.

• 러너 측: policyrunner_flat.py는 ChannelPublisher("rt/lowcmd", LowCmd)로 DDS 토픽 rt/lowcmd에 LowCmd_ 메
  시지를 전송합니다. 0~11번 모터의 tau(PD 토크)만 채워서 publish 합니다.

• 시뮬레이터 측: simulatepython/unitree_mujoco.py가 시작될 때 ChannelFactoryInitialize(config.DOMAIN_ID,
  config.INTERFACE) 호출 후 UnitreeSdk2Bridge를 생성합니다. UnitreeSdk2Bridge 내부(simulate_python/
  unitree_sdk2py_bridge.py)에서 self.low_cmd_suber = ChannelSubscriber("rt/lowcmd", LowCmd)를 등록하고, 콜
  백 LowCmdHandler에서 수신한 명령을 MuJoCo에 반영합니다.

• 적용 방식(simulate_python/unitree_sdk2py_bridge.py: LowCmdHandler):

  mj_data.ctrl[i] = (
  msg.motor_cmd[i].tau
  + msg.motor_cmd[i].kp (msg.motor_cmd[i].q - mj_data.sensordata[i])
  + msg.motor_cmd[i].kd (msg.motor_cmd[i].dq - mj_data.sensordata[i + num_motor])
  )
  즉, 수신한 토크/PD값으로 actuator 입력(mj_data.ctrl)을 계산해 바로 MuJoCo 모델에 적용합니다.

• 인터페이스/도메인: 시뮬레이터는 simulate_python/config.py의 INTERFACE(기본 lo, UNITREE_IFACE로 오버라이드
  가능)와 DOMAIN_ID(기본 1)로 초기화합니다. 러너가 동일한 NIC/도메인(lo, 1)으로 ChannelFactoryInitialize를
  호출하면 DDS 통신이 맞물려 시뮬레이터가 명령을 정상 수신합니다.

---

• 현재 policy_runner_flat.py의 칼만 필터 흐름은 다음과 같습니다.

1. 상태/잡음 모델

• 상태 x: 몸체 좌표계의 선속도 3축 [vx, vy, vz].
• 예측식: x = x + dt * a_body, 공분산 P = P + Q (여기선 F=I).
• 과정잡음 Q: process_var * I (기본 0.2). 가속도 모델의 불확실성을 반영.
• 측정잡음 R: meas_var * I (기본 0.05). sport 속도 측정(SportModeState)을 얼마나 신뢰할지 설정.
• 초기공분산 P0: init_var * I (기본 0.5).

2. 예측 단계 (IMU로 적분)

• 입력 imu_acc는 IMU가 보고하는 특정힘(specific force) → 중력을 더해 실제 선가속도로 변환:
  accel_body = imu_acc + R_w_b @ [0, 0, -9.81]
  (R_w_b는 월드→바디 회전, 쿼터니언에서 계산)
• 속도 적분: x = x + dt * accel_body
• 공분산 업데이트: P = P + Q

3. 갱신 단계 (측정 융합)

• 측정 z: SportModeState의 선속도(바디 프레임). 없으면 스킵.
• 칼만 이득 K = P H^T (H P H^T + R)^{-1}, 여기서 H=I.
• 보정: x = x + K (z - x)
• 공분산: P = (I - K H) P
• prior(갱신 전)와 측정 사이의 오차를 누적해 mean/RMSE를 출력(튜닝 피드백).

4. 스무딩

• 이동평균(MovingAverage, 기본 20샘플)으로 x를 후처리해 정책에 사용할 속도를 안정화.

5. 정책 입력 선택

• --estimate kalman이면 칼만 스무딩 값을 사용.
• --estimate sport이면 측정이 있으면 sport, 없으면 칼만 값을 사용.
• 둘 다 항상 계산/로그되어 비교할 수 있음.

튜닝 포인트/개선 아이디어

• process_var ↑ → 가속도 적분을 더 믿음(빠른 적응, 노이즈 민감). meas_var ↓ → sport 측정을 더 믿음.
• 중력/쿼터니언이 노이즈가 크다면 공분산을 더 키워 관성 적분의 신뢰를 낮추거나, 관측잡음을 키워 측정 의존도를 줄일
  수 있음.
• 속도 드리프트가 크면 스무딩 윈도우를 늘리거나, 가속도 오프셋 보정(예: imu_acc 바이어스) 항을 추가하는 6D 상태
  (x,bias) 모델로 확장 가능.
• 지면 접촉 정보를 활용한 관측(발 Jacobian 기반 속도) 항을 추가하면 저속/정지 구간에서 더 안정화됨.

• 현재 구현한 칼만 필터(SimpleKalman3D)의 모델은 다음과 같습니다.

• 상태(state) x: 몸체 좌표계 선속도 3축 [vx, vy, vz].
• 제어입력 u: 몸체 좌표계 선가속도 accel_body (IMU 특정힘 + 중력 보정), dt로 적분해 속도 예측에 사용.
• 상태전이(F): 단위행렬 I(정적 속도 모델). 예측식은 x ← x + dt u (F@x + dtu).
• 공정잡음(Q): process_var * I (기본 0.2). 가속도 모델/적분 오차를 흡수.
• 측정 z: SportModeState가 주는 몸체 선속도(사용 가능할 때만).
• 측정행렬(H): 단위행렬 I (측정이 속도 자체이므로 직접 관측).
• 측정잡음(R): meas_var * I (기본 0.05). Sport 측정 신뢰도 설정.
• 초기공분산(P0): init_var * I (기본 0.5).

예측 단계:

1. 쿼터니언 → 월드→바디 회전행렬 R_w_b.
2. gravity_body = R_w_b @ [0,0,-9.81]
3. accel_body = imu_acc + gravity_body
4. x = x + dt * accel_body, P = P + Q.

갱신 단계(측정 있을 때만):

• 이득 K = P (P + R)^{-1}, 보정 x = x + K (z - x), 공분산 P = (I - K)P.

후처리: 이동평균 스무딩(기본 20샘플)으로 정책 입력을 안정화.

---

aliengo 자코비안

● 칼만 필터 구현 코드를 분석했습니다. 자코비안 계산과 발 접촉 판단 부분이 모두 포함되어 있습니다. 각 부분을 상세히
설명드리겠습니다.

1. 발 접촉 판단 (Foot Contact Detection)

aliengo_robot_velocity_estimator.py:98-100
foot_contact = self.robot.GetFootContacts()
for leg_id in range(4):
    if foot_contact[leg_id]:

설명:

• 4개의 다리에 대해 각각 지면 접촉 여부를 확인합니다
• 접촉한 발만 속도 추정에 사용합니다 (이유: 공중에 떠있는 발은 신뢰할 수 없는 데이터를 제공)
• GetFootContacts()는 boolean 배열 [FR, FL, RR, RL]을 반환합니다

2. 자코비안 계산 (Jacobian Computation)

aliengo_robot_velocity_estimator.py:101-109
jacobian = self.robot.ComputeJacobian(leg_id)

# Only pick the jacobian related to joint motors
joint_velocities = self.robot.motor_velocities[leg_id * 3:(leg_id + 1) * 3]
leg_velocity_in_base_frame = jacobian.dot(joint_velocities)
base_velocity_in_base_frame = -leg_velocity_in_base_frame[:3]

자코비안의 역할:

• 자코비안(Jacobian Matrix): 관절 속도(joint velocities)를 발끝 속도(foot-tip velocity)로 변환하는 선형 변환
  행렬입니다
• 수식: v_foot = J * q_dot (여기서 q_dot은 관절 속도 벡터)

왜 자코비안을 사용하는가?

• 로봇의 관절 각도와 각속도는 엔코더로 직접 측정 가능
• 하지만 로봇 베이스의 속도는 직접 측정 불가능
• 발이 지면에 고정되어 있다면: v_foot = 0 (발은 움직이지 않음)
• 따라서: v_base = -J * q_dot[:3] (베이스 속도는 발 속도의 반대)

코드 분석:

# 1. 각 다리의 3개 관절 속도를 가져옴 (hip, thigh, calf)
joint_velocities = self.robot.motor_velocities[leg_id * 3:(leg_id + 1) * 3]

# 2. 자코비안을 이용해 발끝 속도 계산
leg_velocity_in_base_frame = jacobian.dot(joint_velocities)

# 3. 발이 지면에 고정되어 있으므로, 베이스 속도는 발 속도의 반대
base_velocity_in_base_frame = -leg_velocity_in_base_frame[:3]

3. 칼만 필터의 두 가지 정보원 융합

aliengo_robot_velocity_estimator.py:80-124

3.1 Prediction Step (IMU 가속도 적분)

# IMU 가속도 데이터를 보정하여 속도 예측
sensor_acc = np.array(robot_state.imu.accelerometer)
calibrated_acc = rot_mat.dot(sensor_acc) + np.array([0., 0., -9.85])
self.filter.predict(u=calibrated_acc * delta_time_s)

문제점:

• 가속도를 적분하면 드리프트(drift)가 발생 → 오차 누적

3.2 Update Step (발 접촉 정보로 보정)

# 접촉한 발들의 속도 측정값을 평균
if observed_velocities:
    observed_velocities = np.mean(observed_velocities, axis=0)
    self.filter.update(observed_velocities)

장점:

• 발 접촉 정보는 드리프트가 없음 (발이 땅에 고정되어 있으면 속도 = 0 기준)
• 칼만 필터가 두 정보를 최적으로 융합

4. 추가 스무딩 (Moving Window Filter)

aliengo_robot_velocity_estimator.py:118-124

vel_x = self.moving_window_filter_x.calculate_average(self.filter.x[0])
vel_y = self.moving_window_filter_y.calculate_average(self.filter.x[1])
vel_z = self.moving_window_filter_z.calculate_average(self.filter.x[2])

• 120개 샘플의 이동 평균으로 노이즈를 추가로 제거합니다

요약

구성요소	위치	역할
발 접촉 판단	line 98	지면에 닿은 발만 속도 추정에 사용
자코비안 계산	line 101	관절 속도 → 베이스 속도 변환
칼만 필터	line 39-48	IMU 드리프트와 발 접촉 정보를 융합
이동 평균	line 51-56	최종 노이즈 제거

이 구조는 Contact-Aided Inertial Navigation (CAIN) 방식으로, 로봇 공학에서 매우 일반적인 접근법입니다.

---

선형 KF

• robot_policy_runner_kalman.py:64-144은 3D 선속도만 상태로 두는 완전 선형 KF입니다. H=I이고 update도 단순하게 z- x를 쓰므로 자코비안이나 EKF/UKF 선형화는 전혀 사용하지 않습니다. “자코비안을 이용해 추정”하는 구조는 아닙니다.
• step()에서 가속도 보정이 accel_body = imu_accel + gravity_body로 되어 있음 (robot_policy_runner_kalman.py:131). 대부분의 IMU는 정지 시 ±9.81을 내보내므로, 선속도를 얻으려면 그 값을 빼서 0에 가깝게 만들어야 합니다. 현재는 중력 방향으로 가속도를 더
  해 버려 정지 시에도 큰 바이어스가 생길 수 있습니다.
• 공분산 예측이 P = P + Q로만 이루어져 있고 dt나 상태 전이 행렬을 반영하지 않습니다 (robot_policy_runner_kalman.py:79-82). 루프가 느려지거나 빨라져도 불확실도가 동일하게 증가하여 튜닝이 흔들릴 수 있습니다.
• SportMode 측정이 없으면(또는 끊기면) 필터는 IMU 적분 + 이동평균으로만 속도를 내므로 드리프트가 크게 누적됩니다 (robot_policy_runner_kalman.py:360-381). 이 경우 “칼만으로 안정적 속도”라고 보기 어렵습니다.
• 추정치가 policy에 공급되는 것은 KF+이동평균 결과(policy_vel = kalman_smooth)라서 측정이 있든 없든 동일한 흐름입니다. SportMode 속도가 들어오면 갱신에 사용되지만, 갱신 노이즈 R와 중력 보정, Q 스케일이 맞지 않으면 여전히 튈 수 있습니다.

정리하면: 이 구현은 자코비안 기반 EKF가 아니라 단순 선형 KF이며, 중력 보정 부호/크기와 Q/R·dt 처리 때문에 선속도가 “잘 나올”지는 보장되지 않습니다. 최소한 정지 상태에서 수 초간 출력이 0 m/s 근처인지 확인하며 중력 보정 부호를 맞추고, Q를 dt에 따라
스케일링하거나 상태 전이를 명시적으로 넣는 것이 필요합니다.

⚠️ 문제점

1. SportModeState 의존성

• Line 243-249: SportModeState에서만 속도를 가져옴
• 문제: 스포츠 모드가 아니면 sport_vel = None
• 결과: measurement 없이 IMU만으로 추정 → 심각한 드리프트 발생

2. 자코비안 기반 측정이 없음

전체 파일에서 검색 결과:

• ❌ jacobian 단어 없음
• ❌ foot_contact 없음
• ❌ mj_jac 없음
• ❌ ComputeJacobian 없음
• ❌ 관절 속도 기반 계산 없음

3. Low-level 모드에서 작동 불가

# Line 254-257: SportModeState가 없으면 Kalman만 사용
if self.estimate_source == "kalman" or sport_vel is None:
    policy_vel = kalman_vel.copy()  # ← measurement 없는 추정값!
else:
    policy_vel = sport_vel.copy()

문제: Low-level 제어 모드에서는 sport_vel = None이므로 drift가 계속 누적됩니다.

---