Discrete Kalman filter

Kalman filter

A wind turbine is modeled under the assumption that the rotor is directly attached to the generator and that the generator torque can be controlled directly. Then we can write (without noise):
\dot\omega_\mathrm{r} = \frac{T_\mathrm{a}-T_\mathrm{g}}{J_\mathrm{r}},
where \omega_\mathrm{r} is the rotor speed, T_\mathrm{a} the aerodynamic torque, T_\mathrm{g} the generator torque and J_\mathrm{r} the combined inertia of the rotor and the generator.

Model equations

\left[ \begin{array}{ccc} \dot\omega_\mathrm{r} \\ \dot{T}_\mathrm{a} \\ \end{array} \right] = \left[ \begin{array}{ccc} 0 & 1/J_\mathrm{r} \\ 0 & 0 \\ \end{array} \right] \left[ \begin{array}{ccc} \omega_\mathrm{r} \\ {T}_\mathrm{a} \\ \end{array} \right] + \left[ \begin{array}{ccc} -1/J_\mathrm{r} \\ 0 \\ \end{array} \right] T_\mathrm{g}+ \left[ \begin{array}{ccc} w_{\omega_\mathrm{r}} \\ w_\mathrm{T_\mathrm{a}} \\ \end{array} \right],
where w_{\omega_\mathrm{r}} and w_\mathrm{T_\mathrm{a}} are the process noise of \omega_\mathrm{r} and \mathrm{T_\mathrm{a}} respectively.

The output equation is defined by
y = \omega_\mathrm{r} + v_\mathrm{\omega_\mathrm{r}},
where v_\mathrm{\omega_\mathrm{r}} is the measurement noise.

Question:
How can I transform this system into a discrete time system and determine the matrices A, B, C and D that I need for implementing it with LowLevelParticleFilters.jl ?

Input will be T_\mathrm{g} and the noisy measurement of \omega_\mathrm{r}, output filtered estimates for T_\mathrm{a} and \omega_\mathrm{r}.

Without having actually used the package, I’m just going to try to rewrite the equations here in the hope that it can help you see the problem in a different way and make it easier for you to reach the solution.

I would write the continuous version of the differential equation for \omega_r as
d\omega_r(t) = \frac{T_a - T_g}{J_r}dt + dw_{\omega_r}(t)

For this stochastic differential equation we can use different approximating numerical schemes, but if we stick with the basic Euler method we can write it as
\omega_r(t+\delta t) \approx \omega(t) + \frac{T_a - T_g}{J_r}\delta t + w_{\omega_r}(t + \delta t) - w_{\omega_r}(t)

In this case you can define a random variable Z(\delta t) that is supposed to represent the increment distribution of the noise: w_{\omega_r}(t + \delta t) - w_{\omega_r}(t), for which the distribution depends on the elapsed time \delta t (the time step).

Would this help?

1 Like

Have you seen Discretization · LowLevelParticleFilters Documentation?

1 Like

No, I overlooked that. Thanks for pointing out!

OK, I am on step further. I wrote the following code:

using ControlSystemsBase, LowLevelParticleFilters
function kalman()
    J = 4.047e7

    A = [0 1/J; 0 0]
    B = [-1/J; 0]
    C = [1 0]
    D = 0

    Ts = 0.1

    sys = ss(A,B,C,D)
    sysd = c2d(sys, Ts) # Discretize the dynamics

    σ1 = 0.5
    N  = σ1*[1; 0]
    sys_w  = ss(A,N,C,D)
    sys_wd = c2d(sys_w, Ts) # Discretize the noise system
    Nd  = sys_wd.B          # The discretized noise input matrix
    R1 = Nd*Nd'            # The final discrete-time covariance matrix
    R2 = [1;;]

    kf = KalmanFilter(sysd.A, sysd.B, sysd.C, sysd.D, R1, R2)
 end

It fails with the error message:

julia> kalman()
ERROR: PosDefException: matrix is not positive definite; Cholesky factorization failed.
Stacktrace:
  [1] checkpositivedefinite
    @ LinearAlgebra ~/.julia/juliaup/julia-1.10.0+0.x64.linux.gnu/share/julia/stdlib/v1.10/LinearAlgebra/src/factorization.jl:67 [inlined]
  [2] cholesky!(A::Hermitian{Float64, Matrix{Float64}}, ::NoPivot; check::Bool)
    @ LinearAlgebra ~/.julia/juliaup/julia-1.10.0+0.x64.linux.gnu/share/julia/stdlib/v1.10/LinearAlgebra/src/cholesky.jl:269
  [3] cholesky!
    @ LinearAlgebra ~/.julia/juliaup/julia-1.10.0+0.x64.linux.gnu/share/julia/stdlib/v1.10/LinearAlgebra/src/cholesky.jl:267 [inlined]
  [4] cholesky!(A::Matrix{Float64}, ::NoPivot; check::Bool)
    @ LinearAlgebra ~/.julia/juliaup/julia-1.10.0+0.x64.linux.gnu/share/julia/stdlib/v1.10/LinearAlgebra/src/cholesky.jl:301
  [5] cholesky! (repeats 2 times)
    @ ~/.julia/juliaup/julia-1.10.0+0.x64.linux.gnu/share/julia/stdlib/v1.10/LinearAlgebra/src/cholesky.jl:295 [inlined]
  [6] cholesky(A::Matrix{Int64}, ::NoPivot; check::Bool)
    @ LinearAlgebra ~/.julia/juliaup/julia-1.10.0+0.x64.linux.gnu/share/julia/stdlib/v1.10/LinearAlgebra/src/cholesky.jl:401 [inlined]
  [7] cholesky (repeats 2 times)
    @ ~/.julia/juliaup/julia-1.10.0+0.x64.linux.gnu/share/julia/stdlib/v1.10/LinearAlgebra/src/cholesky.jl:401 [inlined]
  [8] PDMats.PDMat(mat::Matrix{Float64})
    @ PDMats ~/.julia/packages/PDMats/cAM9h/src/pdmat.jl:19
  [9] MvNormal
    @ ~/.julia/packages/Distributions/UaWBm/src/multivariate/mvnormal.jl:201 [inlined]
 [10] MvNormal
    @ ~/.julia/packages/Distributions/UaWBm/src/multivariate/mvnormal.jl:218 [inlined]
 [11] KalmanFilter(A::Matrix{Float64}, B::Vector{Float64}, C::Matrix{Int64}, D::Int64, R1::Matrix{Float64}, R2::Matrix{Int64})
    @ LowLevelParticleFilters ~/.julia/packages/LowLevelParticleFilters/rO2as/src/kalman.jl:42
 [12] kalman()
    @ Main ~/repos/WindSpeedEstimators/src/kalman.jl:24
 [13] top-level scope
    @ REPL[1]:1

Any idea?

In addition, which matrix is not positive definite but has to be?

UPDATE:
The following lines cause (inside KalmanFilter) the problem:

julia> using Distributions
julia> MvNormal(R1)
ERROR: PosDefException: matrix is not positive definite; Cholesky factorization failed.
julia> R1
2×2 Matrix{Float64}:
 0.0025  0.0
 0.0     0.0

But why?

You can solve this problem by either

  • Adding a tiny scalar times the identity matrix to R_1
  • Use the Square-root Kalman filter SqKalmanFilter type. This type does not have a user-friendly constructor that accepts pre-constructed Cholesky factors, so the option above is simpler to get you started.
1 Like

Looking a bit closer at your system definition, the system you have posted is non-minimal (not controllable), with both a pole and a zero in the origin. Is this expected?

Further, you mention in your first post that you have two process-noise sources, w_{\omega_r} and w_{T_a}, this is not reflected in your modeled continuous-time covariance matrix, which would for such a setup be diagonal and non-singular

2 Likes

The system definition is a simplified version of Eq. (4) in [1]. They write:
“The … model given by (2) is further augmented by appending the Ta as an additional state.”

I assume that augmenting the model made it non-minimal, and I think it was done to take the noise
of Ta into account.

The estimator is using \omega and T_r as noisy inputs and estimates the real \omega.

[1] Kalman Filter-based Wind Speed Estimation for Wind Turbine Control

What is the continues-time covariance matrix in the following piece of code:

σ1 = 0.5
N  = σ1*[1; 0]
sys_w  = ss(A,N,C,D)
sys_wd = c2d(sys_w, Ts) # Discretize the noise system
Nd  = sys_wd.B          # The discretized noise input matrix
R1 = Nd*Nd'             # The final discrete-time covariance matrix
R2 = [1;;]

It cannot be N, because N is a column vector, I cannot replace it with a diagonal matrix…

N*N'

In the model you have at the moment, you are assuming that the dynamics of the second state variable is completely deterministic, this is what is causing the singular exception. Does this make sense? If so, what is the reason for adding this state variable rather than treating it as a known input? If you look through your equaitons, there is nothing at all affecting the second state variable, the seond row of A is all zeros, and both B[2] and N[2] are zero.

Well, Ta is not known, the only thing we know about Ta is the mean and the variance. We
want to estimate it based on omega and Tg, which are the (noisy) inputs…

So why does the covariance matrix not reflect that T_a is affected by noise?

you only model noise acting on \omega_r.

I use now this code:

using ControlSystemsBase
import LowLevelParticleFilters as ll

J = 4.047e7
sigma_p = 0.01  # process noise
sigma_m = 1000  # measurement noise
Ts = 0.1        # sampling time [s]

A = [0 1/J; 0 0]
B = [-1/J; 0;;]
C = [1 0]
D = [0;;]

sys = ss(A, B, C, D)    # Define the linear system
sysd = c2d(sys, Ts)     # Discretize the dynamics

N  =  sigma_p * [1; 1;;]
sys_w  = ss(A, N, C, D) # Define the noise system
sys_wd = c2d(sys_w, Ts) # Discretize the noise system
Nd  = sys_wd.B          # The discretized noise input matrix
R1 = Nd*Nd' + 1e-6*I    # The final discrete-time covariance matrix
R2 = [sigma_m;;]

kf = ll.KalmanFilter(sysd.A, sysd.B, sysd.C, sysd.D, R1, R2)

Did I derive R1 and R2 now correctly, assuming “the process and measurement noises are set to 10e-2 and 1000, respectively”?

Not quite, this

N  =  sigma_p * [1; 1;;]

indicates that the same source of noise acts on both state variables. What you probably intend to write is

N  =  sigma_p * I(2)

indicating that there are two independent sources of dynamics noise, each affecting one of the state variables. If this is what you mean, you have a full-rank covariance matrix which can be discretized directly without ZoH assumption using

R1c = sigma_p^2 * I(2) # Notice the square here since this is a covariance matrix
R1d = c2d(sysd, R1c, Ts) # Discrete version of R1

A second point is that the two state variables are describing two completely different things, so it would probably make sense to give them unique variances, rather than a single variance parameter sigma_p^2 for both of them, the state variables do not even have the same unit so neither does the noise acting on them :slight_smile:

Next try:

using ControlSystemsBase
import LowLevelParticleFilters as ll

J  = 4.047e7      # total inertia on the side of the rotor
sigma_p = 0.01    # process noise
sigma_m = 1000    # measurement noise
Ts = 0.1          # sampling time [s]

A = [0 1/J; 0 0]
B = [-1/J; 0;;]
C = [1 0]
D = [0;;]

sys = ss(A,B,C,D)       # linear system
sysd = c2d(sys, Ts)     # discretize the dynamics

R1c = sigma_p^2 * I(2)  # linear covariance matrix
R1d = c2d(sys, R1c, Ts) # discretized covariance matrix
R2d = [sigma_m;;]

kf = ll.KalmanFilter(sysd.A, sysd.B, sysd.C, sysd.D, R1d, R2d)

Is this correct now?

I think you had a typo in this line: R1d = c2d(sysd, R1c, Ts) . The c2d function does not accept a discrete system as input.

You further wrote: “A second point is that the two state variables are describing two completely different things, so it would probably make sense to give them unique variances, rather than a single variance parameter…”

I completely agree, but in the moment I just try to reproduce the results of the paper mentioned above, and they used a single variance parameter.

Looks alright

1 Like

Extra question:
I use:

    kf.x .= [omega_0, ta_0]

to define the initial state of the Kalman filter. Is that a good way to do it, or am I using an internal property of the filter object that can change in future versions and there is a better way of doing it?

The intended way is to provide the distribution of the initial state, you do this by providing the d0 argument here, for example

d0 = MvNormal(x0, R0)

And what would be R0?

The covariance matrix of the initial state estimate