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)

Explanation:
Measurement Function:
The function measurement_function(x) computes the expected measurement by adding the sensor offset to the reference position
x[0:3].

Jacobian Function:
The function measurement_jacobian(x) returns an identity matrix here because the derivative of x+offset with respect to x is simply the identity. In more complex models (e.g., including rotational dynamics), the Jacobian would be more complicated.

Filter Update:
The EKF uses these functions during the update step to compare the actual measurement with the expected measurement and correct the state accordingly.

@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