1
  2
  3
  4
  5
  6
  7
  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144

#[macro_use(azip)]
extern crate ndarray;

use ndarray::prelude::*;
use ndarray::Data;
use ndarray::linalg::{
    general_mat_mul,
    general_mat_vec_mul,
};

/// The parameters of recursive least squares algorithm.
///
/// This struct contains all parameters involved in a recursive
/// least squares algorithm with exponential forgetting. For a review see
/// [Haykin's Adaptive Filter Theory][http://www.isbnsearch.org/isbn/9780132671453].
/// The implementation here does not implicitly take time into account. By making a choice of the
/// forgetting factor λ < 1 and shifting down old values of the input vector manually, the user can
/// get this algorithm to behave accordingly.
pub struct Rls<F> {

    /// The inverse forgetting factor λ^{-1}.
    inv_forgetting_factor: F,

    /// The gain vector used during the update of the inverse correlation matrix P(i) and the
    /// (tap) weight vector w(i).
    gain: Array1<F>,

    /// The *inverse correlation matrix*, initialized as P = δ^{-1} · 𝟙, with 𝟙 the unit matrix and
    /// δ an initialization factor recommended to be less than ~ 0.01 σ^2, the variance of the data
    /// sample to be analyzed (cf. [Haykin's Adaptive Filter Theory]).
    inverse_correlation: Array2<F>,

    /// The *(tap) weight vector* w(i) at time i used to produce the transversal filter's output
    /// y(i) = w(i) · u(i), where u(i) is the *(tap) input vector* at time i.
    weight: Array1<F>,

    /// The prior error used during the udpate of the inverse correlation matrix P(i) and the
    /// (tap) weight vector w(i). The prior error is calculated as the difference between the
    /// desired output and the filter output before an update.
    prior_error: F,

    // Two scratch matrices used for for update of the inverse correlation matrix.
    temp_a: Array2<F>,
    temp_b: Array2<F>,
}

impl<F: NdFloat> Rls<F> {
    /// Constructs a new Rls object with initialization factor δ and a weight vector of length n.
    pub fn new(initialization_factor: F, forgetting_factor: F, n: usize) -> Self {
        let weight = Array1::zeros(n);

        Rls::with_weight(initialization_factor, forgetting_factor, weight)
    }

    /// Constructs a new Rls object with initialization factor δ and pre-defined weight w.
    pub fn with_weight(initialization_factor: F, forgetting_factor: F, weight: Array1<F>) -> Self {
        let one = F::one();
        let zero = F::zero();

        let n = weight.len();

        let inv_forgetting_factor = one / forgetting_factor;

        let gain = Array1::zeros(n);
        let prior_error = zero;

        let mut inverse_correlation = Array2::eye(n);
        inverse_correlation *= one/initialization_factor;

        let temp_a = Array2::zeros([n,n]);
        let temp_b = Array2::zeros([n,n]);

        Rls {
            inv_forgetting_factor,
            gain,
            inverse_correlation,
            weight,
            prior_error,
            temp_a,
            temp_b,
        }
    }

    /// Performs a recursive update of inverse correlation matrix and weight vector.
    pub fn update<S>(&mut self, input: &ArrayBase<S, Ix1>, target: F)
        where S: Data<Elem = F>
    {
        let one = F::one();
        let zero = F::zero();

        // Update the gain vector.
        general_mat_vec_mul(
            one,
            &self.inverse_correlation,
            input,
            one,
            &mut self.gain
        );
        let c = self.inv_forgetting_factor + self.gain.dot(&self.gain);
        self.gain /= c;

        // Calculate the prior error using the not yet updated tap weight.
        self.prior_error = target - self.weight.dot(&input);

        // Update the tap weight.
        self.weight.scaled_add(self.prior_error, &self.gain);

        azip!(mut row (self.temp_a.genrows_mut()), gain (&self.gain) in {
            azip!(mut row, input (input) in {
                *row = gain * input;
        })});

        general_mat_mul(one, &self.temp_a, &self.inverse_correlation, zero, &mut self.temp_b);
        self.inverse_correlation -= &self.temp_b;
        self.inverse_correlation *= self.inv_forgetting_factor;
    }
}

impl<T> Rls<T> {
    /// Returns a reference to the gain vector.
    pub fn gain_ref(&self) -> &Array1<T> {
        &self.gain
    }

    /// Returns a reference to the inverse correlation matrix.
    pub fn inverse_correlation_ref(&self) -> &Array2<T> {
        &self.inverse_correlation
    }

    /// Returns a reference to the inverse forgetting factor.
    pub fn inv_forgetting_factor_ref(&self) -> &T {
        &self.inv_forgetting_factor
    }

    /// Returns a reference to the (tap) weight vector.
    pub fn weight_ref(&self) -> &Array1<T> {
        &self.weight
    }

    /// Returns a refernce to the prior error.
    pub fn prior_error_ref(&self) -> &T {
        &self.prior_error
    }
}