multiple_sensor.py

import numpy as np
from filterpy.kalman import ExtendedKalmanFilter

# Assume our state vector x includes only position for simplicity.
# In practice, it would include orientation, velocity, etc.
# Let’s say x[0:3] is the "true" position of the reference point.

# Known sensor offset relative to the reference point
sensor_offset = np.array([0.1, -0.05, 0.0])  # in meters

def measurement_function(x):
    """
    Measurement function h(x) that outputs expected sensor measurement.
    For example, the sensor measures the position of its mounting point,
    which is the body position plus the sensor offset.
    """
    # x[0:3] is the reference position; add the offset to get the sensor's position.
    return x[0:3] + sensor_offset

def measurement_jacobian(x):
    """
    Compute the Jacobian matrix of h(x) with respect to the state vector x.
    For this simple case, it is just an identity matrix for the position part.
    """
    # Assuming state x has dimension 3 (just position here)
    H = np.eye(3)
    return H

# Create an Extended Kalman Filter instance
ekf = ExtendedKalmanFilter(dim_x=3, dim_z=3)
ekf.x = np.array([0.0, 0.0, 0.0])  # initial state (reference point)
ekf.P = np.eye(3) * 1000           # initial uncertainty
ekf.F = np.eye(3)                  # state transition (for a static example)
ekf.R = np.eye(3) * 5              # measurement noise covariance
ekf.Q = np.eye(3) * 0.1            # process noise covariance

# Override the measurement function and Jacobian for the EKF
ekf.h = measurement_function
ekf.H_j = measurement_jacobian

# Simulated sensor measurement (e.g., measured position at the sensor location)
z = np.array([0.95, -0.05, 0.0])  # example measurement

# Prediction step
ekf.predict()

# Update step with the sensor measurement
ekf.update(z, HJacobian=measurement_jacobian, Hx=measurement_function)

print("Updated state estimate (reference position):", ekf.x)

@yoi-hibino Markdownが使えます

あ

ウェーイ

死んだ
強調

printf("Unko.");

$ ls

• ラーメン
• 餃子

mermaid も使える

sequenceDiagram
    actor Alice
    participant Source 1
    participant Source 2
    participant Service A
    participant Database A

    Alice->>Source 1: 情報1取得
    Source 1->>Alice: 
    Alice->>Source 2: 情報2取得
    Source 2->>Alice: 
    Alice->>Alice: 結果=情報1+情報2
    Alice->>Service A: 結果格納
    Service A->>Database A: 結果格納

Loading

solid cube_corner
  facet normal 0.0 -1.0 0.0
    outer loop
      vertex 0.0 0.0 0.0
      vertex 1.0 0.0 0.0
      vertex 0.0 0.0 1.0
    endloop
  endfacet
  facet normal 0.0 0.0 -1.0
    outer loop
      vertex 0.0 0.0 0.0
      vertex 0.0 1.0 0.0
      vertex 1.0 0.0 0.0
    endloop
  endfacet
  facet normal -1.0 0.0 0.0
    outer loop
      vertex 0.0 0.0 0.0
      vertex 0.0 0.0 1.0
      vertex 0.0 1.0 0.0
    endloop
  endfacet
  facet normal 0.577 0.577 0.577
    outer loop
      vertex 1.0 0.0 0.0
      vertex 0.0 1.0 0.0
      vertex 0.0 0.0 1.0
    endloop
  endfacet
endsolid

Loading