Dyad

Appearance

LIBRARY
SqKalmanObserver.md

SqKalmanObserver ​

Time-varying Kalman filter in square-root form (LowLevelParticleFilters.jl SqKalmanFilter) estimating the state of the discrete-time linear model

Misplaced &

run as a clocked component. At every tick the prediction x(t|t-1) is corrected with the measurements that are available, the filtered estimate x(t|t) is output as xhat, and the control u applied at this tick is used to predict the estimate for the next tick. A measurement is used when it is finite and, with use_valid, when its entry of y_valid is true; the others are left out of the correction (the rows of C, D and the block of R2 of the available measurements are used), so that a sensor that drops out does not bias the estimate. Without available measurements the estimate is only predicted and its covariance grows.

The covariance is carried as the upper-triangular Cholesky factor R (covariance R'R), as in SqKalmanFilter, and both updates are QR factorizations, so it stays symmetric and positive semidefinite in finite precision. The notation is that of LowLevelParticleFilters' forward_trajectory: x, R are the prediction x(t|t-1) and its factor, xt, Rt the filtered estimate x(t|t) and its factor; sqkf_covariance forms the covariance from a carrier.

Measurements must be sampled before this component. With a nonzero D the correction uses the control of the tick, so the control must not be computed from xhat of the same tick.

Tunable parameters ​

R1, R2, x0 and R0 are ordinary parameters: the covariances are read at every tick, the initial estimate at initialization. A, B, C and D are structural. The filter runs in Julia and is not exported to C; use LinearMPCObserver with a stationary gain for C export.

Usage ​

MPCComponents.SqKalmanObserver(R1=_sqkf_R1(kf), R2=_sqkf_R2(kf), x0=_sqkf_x0(kf), R0=R1, xR0=sqkf_carrier0(x0, R0), u_dummy=fill(0.0, nu), valid_all=fill(1.0, ny))

Parameters: ​

NameDescriptionUnitsDefault value
AState matrix (nx × nx)–fill(0.0, 0, 0)
BInput matrix (nx × nu)–fill(0.0, 0, 0)
CMeasurement matrix (ny × nx)–fill(0.0, 0, 0)
DFeedthrough from the control to the measurements (ny × nu); empty for zero–fill(0.0, 0, 0)
use_validWhether the y_valid input exists; without it, every finite measurement is used–false
kfSet-up filter built from the parameters above; pass build_sqkf(...) to bypass them–build_sqkf(..., C=C, D=D)
nxState dimension (derived from kf)–_sqkf_nx(kf)
nuNumber of control inputs (derived from kf)–_sqkf_nu(kf)
nyNumber of measurements (derived from kf)–_sqkf_ny(kf)
n_validWidth of the y_valid input: ny with use_valid, else 0 (derived)–(use_valid) ? (ny) : (0)
has_DWhether D is nonzero, so that the correction uses the control of the tick (derived from kf)–_sqkf_has_D(kf)
ncLength of the carriers [x; vec(R)], nx + nx² (derived)–nx + nx ^ 2
R1Process-noise covariance (nx × nx, e.g. diagonal([1e-4, 1e-4])); tunable–_sqkf_R1(kf)
R2Measurement-noise covariance (ny × ny); tunable–_sqkf_R2(kf)
x0Initial state estimate; tunable, read at initialization–_sqkf_x0(kf)
R0Covariance of the initial state estimate (nx × nx), by default R1; tunable, read at initialization–R1

Connectors ​

  • y - This connector represents a real signal as an input to a component (RealInput)

  • y_valid - This connector represents a boolean signal as an input to a component (BooleanInput)

  • u - This connector represents a real signal as an input to a component (RealInput)

  • xhat - This connector represents a real signal as an output from a component (RealOutput)

Variables ​

NameDescriptionUnits
xRClocked state: the prediction x(t+1|t) and the upper-triangular factor of its covariance (column-major)–
xRtThe filtered estimate x(t|t) and the upper-triangular factor of its covariance (column-major)–
u_corrControl used by the correction–

Behavior ​

Behavior of this component cannot be rendered because it includes path variables.

Source ​

dyad
"""
Time-varying Kalman filter in square-root form (LowLevelParticleFilters.jl `SqKalmanFilter`)
estimating the state of the discrete-time linear model

```math
\\begin{aligned}
x(t+1) &= A x(t) + B u(t) + w(t), \\quad w \\sim N(0, R_1) \\\\
y(t) &= C x(t) + D u(t) + e(t), \\quad e \\sim N(0, R_2)
\\end{aligned}
```

run as a clocked component. At every tick the prediction `x(t|t-1)` is corrected with the
measurements that are available, the filtered estimate `x(t|t)` is output as `xhat`, and the
control `u` applied at this tick is used to predict the estimate for the next tick. A measurement
is used when it is finite and, with `use_valid`, when its entry of `y_valid` is true; the others
are left out of the correction (the rows of `C`, `D` and the block of `R2` of the available
measurements are used), so that a sensor that drops out does not bias the estimate. Without
available measurements the estimate is only predicted and its covariance grows.

The covariance is carried as the upper-triangular Cholesky factor `R` (covariance `R'R`), as in
`SqKalmanFilter`, and both updates are QR factorizations, so it stays symmetric and positive
semidefinite in finite precision. The notation is that of LowLevelParticleFilters'
`forward_trajectory`: `x`, `R` are the prediction `x(t|t-1)` and its factor, `xt`, `Rt` the filtered
estimate `x(t|t)` and its factor; `sqkf_covariance` forms the covariance from a carrier.

Measurements must be sampled before this component. With a nonzero `D` the correction uses the
control of the tick, so the control must not be computed from `xhat` of the same tick.

# Tunable parameters
`R1`, `R2`, `x0` and `R0` are ordinary parameters: the covariances are read at every tick, the initial
estimate at initialization. `A`, `B`, `C` and `D` are structural. The filter runs in Julia and is not
exported to C; use `LinearMPCObserver` with a stationary gain for C export.
"""
component SqKalmanObserver@[input clk extends Discrete]
  "Sampled measurements; non-finite values are not used"
  y = [RealInput@[clk]() for i in 1:ny] {
    "Dyad": {"placement": {"diagram": {"x1": -100, "y1": 700, "x2": 0, "y2": 800}}}
  }
  "Validity of the measurements: y[i] is used when y_valid[i] is true (only with use_valid)"
  y_valid = [BooleanInput@[clk]() for i in 1:n_valid] if use_valid {
    "Dyad": {"placement": {"diagram": {"x1": 450, "y1": 1000, "x2": 550, "y2": 1100}}}
  }
  "Control applied at the current tick, used to predict the estimate for the next tick"
  u = [RealInput@[clk]() for i in 1:nu] {
    "Dyad": {"placement": {"diagram": {"x1": -100, "y1": 200, "x2": 0, "y2": 300}}}
  }
  "State estimate filtered with the measurements of the current tick, x(t|t)"
  xhat = [RealOutput@[clk]() for i in 1:nx] {
    "Dyad": {"placement": {"diagram": {"x1": 1000, "y1": 450, "x2": 1100, "y2": 550}}}
  }
  "State matrix (nx × nx)"
  structural parameter A::Real[:, :] = fill(0.0, 0, 0)
  "Input matrix (nx × nu)"
  structural parameter B::Real[:, :] = fill(0.0, 0, 0)
  "Measurement matrix (ny × nx)"
  structural parameter C::Real[:, :] = fill(0.0, 0, 0)
  "Feedthrough from the control to the measurements (ny × nu); empty for zero"
  structural parameter D::Real[:, :] = fill(0.0, 0, 0)
  "Whether the y_valid input exists; without it, every finite measurement is used"
  structural parameter use_valid::Boolean = false
  "Set-up filter built from the parameters above; pass build_sqkf(...) to bypass them"
  structural parameter kf::SqKFRef = build_sqkf(A = A, B = B, C = C, D = D)
  "State dimension (derived from kf)"
  final structural parameter nx::Integer = _sqkf_nx(kf)
  "Number of control inputs (derived from kf)"
  final structural parameter nu::Integer = _sqkf_nu(kf)
  "Number of measurements (derived from kf)"
  final structural parameter ny::Integer = _sqkf_ny(kf)
  "Width of the y_valid input: ny with use_valid, else 0 (derived)"
  final structural parameter n_valid::Integer = use_valid ? ny : 0
  "Whether D is nonzero, so that the correction uses the control of the tick (derived from kf)"
  final structural parameter has_D::Boolean = _sqkf_has_D(kf)
  "Length of the carriers [x; vec(R)], nx + nx² (derived)"
  final structural parameter nc::Integer = nx + nx ^ 2
  "Process-noise covariance (nx × nx, e.g. diagonal([1e-4, 1e-4])); tunable"
  parameter R1::Real[nx, nx] = _sqkf_R1(kf)
  "Measurement-noise covariance (ny × ny); tunable"
  parameter R2::Real[ny, ny] = _sqkf_R2(kf)
  "Initial state estimate; tunable, read at initialization"
  parameter x0::Real[nx] = _sqkf_x0(kf)
  "Covariance of the initial state estimate (nx × nx), by default R1; tunable, read at initialization"
  parameter R0::Real[nx, nx] = R1
  "Initial carrier: the prior x0 as the prediction for the first tick and the factor of R0"
  final parameter xR0::Real[nc] = sqkf_carrier0(x0, R0)
  "Zero placeholder used as the control of the correction when D is zero"
  final parameter u_dummy::Real[nu] = fill(0.0, nu)
  "Validity used when y_valid is absent: every measurement (nonzero entries are valid)"
  final parameter valid_all::Real[ny] = fill(1.0, ny)
  "Clocked state: the prediction x(t+1|t) and the upper-triangular factor of its covariance (column-major)"
  variable xR::Real[nc]
  "The filtered estimate x(t|t) and the upper-triangular factor of its covariance (column-major)"
  variable xRt::Real[nc]
  "Control used by the correction"
  variable u_corr::Real[nu]
relations
  if has_D
    u_corr = u
  else
    u_corr = u_dummy
  end
  initial xR@(clk-1) = xR0
  if use_valid
    xRt@clk = sqkf_correct(xR@(clk-1), y@clk, y_valid@clk, u_corr@clk, R2, kf)
  else
    xRt@clk = sqkf_correct(xR@(clk-1), y@clk, valid_all, u_corr@clk, R2, kf)
  end
  xR@clk = sqkf_predict(xRt@clk, u@clk, R1, kf)
  for i in 1:nx
    xhat[i] = xRt[i]
  end
metadata {
  "Dyad": {
    "icons": {"default": "dyad://MPCComponents/SqKalmanObserver.svg"},
    "doc": {"behavior": false},
    "panel": {
      "control": "tabs",
      "children": {
        "Model": {"control": "fieldset", "contents": ["A", "B", "C", "D"]},
        "Noise": {"control": "fieldset", "contents": ["R1", "R2", "x0", "R0"]},
        "Measurements": {"control": "fieldset", "contents": ["use_valid"]}
      }
    }
  }
}
end
Flattened Source
dyad
"""
Time-varying Kalman filter in square-root form (LowLevelParticleFilters.jl `SqKalmanFilter`)
estimating the state of the discrete-time linear model

```math
\\begin{aligned}
x(t+1) &= A x(t) + B u(t) + w(t), \\quad w \\sim N(0, R_1) \\\\
y(t) &= C x(t) + D u(t) + e(t), \\quad e \\sim N(0, R_2)
\\end{aligned}
```

run as a clocked component. At every tick the prediction `x(t|t-1)` is corrected with the
measurements that are available, the filtered estimate `x(t|t)` is output as `xhat`, and the
control `u` applied at this tick is used to predict the estimate for the next tick. A measurement
is used when it is finite and, with `use_valid`, when its entry of `y_valid` is true; the others
are left out of the correction (the rows of `C`, `D` and the block of `R2` of the available
measurements are used), so that a sensor that drops out does not bias the estimate. Without
available measurements the estimate is only predicted and its covariance grows.

The covariance is carried as the upper-triangular Cholesky factor `R` (covariance `R'R`), as in
`SqKalmanFilter`, and both updates are QR factorizations, so it stays symmetric and positive
semidefinite in finite precision. The notation is that of LowLevelParticleFilters'
`forward_trajectory`: `x`, `R` are the prediction `x(t|t-1)` and its factor, `xt`, `Rt` the filtered
estimate `x(t|t)` and its factor; `sqkf_covariance` forms the covariance from a carrier.

Measurements must be sampled before this component. With a nonzero `D` the correction uses the
control of the tick, so the control must not be computed from `xhat` of the same tick.

# Tunable parameters
`R1`, `R2`, `x0` and `R0` are ordinary parameters: the covariances are read at every tick, the initial
estimate at initialization. `A`, `B`, `C` and `D` are structural. The filter runs in Julia and is not
exported to C; use `LinearMPCObserver` with a stationary gain for C export.
"""
component SqKalmanObserver
  "Sampled measurements; non-finite values are not used"
  y = [RealInput@[clk]() for i in 1:ny] {
    "Dyad": {"placement": {"diagram": {"x1": -100, "y1": 700, "x2": 0, "y2": 800}}}
  }
  "Validity of the measurements: y[i] is used when y_valid[i] is true (only with use_valid)"
  y_valid = [BooleanInput@[clk]() for i in 1:n_valid] if use_valid {
    "Dyad": {"placement": {"diagram": {"x1": 450, "y1": 1000, "x2": 550, "y2": 1100}}}
  }
  "Control applied at the current tick, used to predict the estimate for the next tick"
  u = [RealInput@[clk]() for i in 1:nu] {
    "Dyad": {"placement": {"diagram": {"x1": -100, "y1": 200, "x2": 0, "y2": 300}}}
  }
  "State estimate filtered with the measurements of the current tick, x(t|t)"
  xhat = [RealOutput@[clk]() for i in 1:nx] {
    "Dyad": {"placement": {"diagram": {"x1": 1000, "y1": 450, "x2": 1100, "y2": 550}}}
  }
  "State matrix (nx × nx)"
  structural parameter A::Real[:, :] = fill(0.0, 0, 0)
  "Input matrix (nx × nu)"
  structural parameter B::Real[:, :] = fill(0.0, 0, 0)
  "Measurement matrix (ny × nx)"
  structural parameter C::Real[:, :] = fill(0.0, 0, 0)
  "Feedthrough from the control to the measurements (ny × nu); empty for zero"
  structural parameter D::Real[:, :] = fill(0.0, 0, 0)
  "Whether the y_valid input exists; without it, every finite measurement is used"
  structural parameter use_valid::Boolean = false
  "Set-up filter built from the parameters above; pass build_sqkf(...) to bypass them"
  structural parameter kf::SqKFRef = build_sqkf(A = A, B = B, C = C, D = D)
  "State dimension (derived from kf)"
  final structural parameter nx::Integer = _sqkf_nx(kf)
  "Number of control inputs (derived from kf)"
  final structural parameter nu::Integer = _sqkf_nu(kf)
  "Number of measurements (derived from kf)"
  final structural parameter ny::Integer = _sqkf_ny(kf)
  "Width of the y_valid input: ny with use_valid, else 0 (derived)"
  final structural parameter n_valid::Integer = use_valid ? ny : 0
  "Whether D is nonzero, so that the correction uses the control of the tick (derived from kf)"
  final structural parameter has_D::Boolean = _sqkf_has_D(kf)
  "Length of the carriers [x; vec(R)], nx + nx² (derived)"
  final structural parameter nc::Integer = nx + nx ^ 2
  "Process-noise covariance (nx × nx, e.g. diagonal([1e-4, 1e-4])); tunable"
  parameter R1::Real[nx, nx] = _sqkf_R1(kf)
  "Measurement-noise covariance (ny × ny); tunable"
  parameter R2::Real[ny, ny] = _sqkf_R2(kf)
  "Initial state estimate; tunable, read at initialization"
  parameter x0::Real[nx] = _sqkf_x0(kf)
  "Covariance of the initial state estimate (nx × nx), by default R1; tunable, read at initialization"
  parameter R0::Real[nx, nx] = R1
  "Initial carrier: the prior x0 as the prediction for the first tick and the factor of R0"
  final parameter xR0::Real[nc] = sqkf_carrier0(x0, R0)
  "Zero placeholder used as the control of the correction when D is zero"
  final parameter u_dummy::Real[nu] = fill(0.0, nu)
  "Validity used when y_valid is absent: every measurement (nonzero entries are valid)"
  final parameter valid_all::Real[ny] = fill(1.0, ny)
  "Clocked state: the prediction x(t+1|t) and the upper-triangular factor of its covariance (column-major)"
  variable xR::Real[nc]
  "The filtered estimate x(t|t) and the upper-triangular factor of its covariance (column-major)"
  variable xRt::Real[nc]
  "Control used by the correction"
  variable u_corr::Real[nu]
relations
  if has_D
    u_corr = u
  else
    u_corr = u_dummy
  end
  initial xR@(clk-1) = xR0
  if use_valid
    xRt@clk = sqkf_correct(xR@(clk-1), y@clk, y_valid@clk, u_corr@clk, R2, kf)
  else
    xRt@clk = sqkf_correct(xR@(clk-1), y@clk, valid_all, u_corr@clk, R2, kf)
  end
  xR@clk = sqkf_predict(xRt@clk, u@clk, R1, kf)
  for i in 1:nx
    xhat[i] = xRt[i]
  end
metadata {
  "Dyad": {
    "icons": {"default": "dyad://MPCComponents/SqKalmanObserver.svg"},
    "doc": {"behavior": false},
    "panel": {
      "control": "tabs",
      "children": {
        "Model": {"control": "fieldset", "contents": ["A", "B", "C", "D"]},
        "Noise": {"control": "fieldset", "contents": ["R1", "R2", "x0", "R0"]},
        "Measurements": {"control": "fieldset", "contents": ["use_valid"]}
      }
    }
  }
}
end


Test Cases ​

No test cases defined.

  • Examples

  • Experiments

  • Analyses