Federated Kalman Filter Generator using Genetic Algorithms

As part of his scientific activity, he implemented the so-called Federated Kalman Filter. This article describes what “Federated FC” is, how it differs from a generalized FC, and also describes a console application that implements this filter and genetic algorithms for selecting parameters of its mathematical model. The application was implemented using TPL (Task Parallel Library), so the post will be interesting not only to specialists in digital signal processing.

UPD1 : after reading two recent articles, I decided to join the experiment / research / adventure too (call it what you want). At the end of the article, I added another poll - “ Would you encourage such narrowly specialized articles on Habrahabr by ruble?”"

Introduction

On a habr already wrote that the Kalman Filter (FC), for what it is necessary and how it is composed (conceptually). Below are some of the articles available.
• Kalman filter
• Kalman filter -! Difficult?
• Kalman filter - Introduction
• On the verge of augmented reality: what should developers prepare for (part 2 of 3)
• Non-orthogonal SINS for small UAVs
• Classical mechanics: on diffusions “on fingers”
There are also a lot of articles on the application of different stochastic algorithms optimization of nonlinear functions of many variables. Here I include evolutionary algorithms, simulated annealing, population algorithm. Links to some of the articles below.
•Annealing Simulation Method
• What is a genetic algorithm?
• Genetic algorithms. From theory to practice
• Genetic algorithms, image recognition
• Genetic algorithm. Just about the complex
• Genetic algorithms for finding solutions and generation
• Genetic algorithm using the Robocode bot example
• Genetic algorithms in MATLAB
• Population algorithm based on the behavior of a school of fish
• Concepts of the practical use of genetic algorithms
• Overview of neural network evolution methods
• TAU Darwinism
•Natural algorithms. Bee swarm behavior algorithm
• Natural algorithms. Implementation of the Bee Swarm Behavior Algorithm
In one of his previous articles ( TAU Darwinism: Ruby Implementation) I presented to your attention the results of using genetic algorithms (GA) for the selection of parameters of a mathematical model of a dynamic system. This article presents the results of using GA in a more complex task - the synthesis of the Kalman Filter model. As part of this task, I “set” genetic algorithms on the dynamics parameters in the transition matrix (aka Transition Matrix) of the FC. GA could also be used to select the values ​​of covariance matrices in FC, but I decided not to do this yet. I generated the noise myself, because I knew the matrix of covariance of measurement errors in advance. The process noise covariance matrix is ​​never exactly known in practice, therefore I chose its value based on the maximum sensitivity of the PC to a change in the values ​​in the transition matrix.
Be that as it may, expanding the problem described here to the case of optimizing the values ​​of the covariance matrices will not be difficult. It will just be necessary to realize another objective function in which covariance matrices will be generated by the values ​​of the individual genes, and the value of the transition matrix will be set in advance. Thus, it will be possible to divide the whole process of synthesis of FC into two stages:
1. Optimization of the parameters of the mathematical model of FC
2. Optimization of the values ​​of covariance matrices The
second stage, as I wrote above, has not yet been implemented. However, its implementation is in the plans.

Formulation of the problem

So the challenge. Suppose we have a certain measuring system consisting of several sensors (measuring channels). As an example, take the non-orthogonal SINS described in another article of mine ( Non-orthogonal SINS for small UAVs ). Each of the sensors of such a system is a separate dynamic system, and at each particular moment in time it produces one scalar measurement. There are eight measuring channels in total (according to the number of sensitivity axes of the accelerometers in the block), and three parameters are sought for (linear acceleration components of the measuring block).
What do we know about sensors? We only have to record signals from sensors in static mode. Those. for us, someone placed a block of accelerometers somewhere in the basement and for, say, a day recorded the values ​​of the projection of gravitational acceleration on the sensitivity axis of the accelerometers. Also, let us be given the approximate value of the matrix of guide cosines of the sensors in the block.
What is required of us? To compile the algorithm of the “Kalman Type Filter” (FCT), which would give optimal estimates of the linear acceleration of the measuring unit. By optimum is meant the minimum variance of estimation errors and the minimum of their expectation (minimum bias of the “zero” errors).

Note: the name “Kalman Type Filter” is used because the classical FC implies that its model coincides with the model of a real object, and the use of another model is already called the “Lewenberger Identification Monitoring Device”, which belongs to the FCT group.

It is proposed to use genetic algorithms to select the parameters of sensor models. The values ​​of the genes will be used as coefficients of the differential equations of the sensors (see “Identification Object” in the TAU-Darwinism article ):

Based on these coefficients, discrete models of sensors in space-states will be built (see “Mathematical Model” in the Kalman Filter - ! complicated? ):


Based on these models, a “private” Kalman type filter will be compiled. An assembly of several such “private” filters forms a federal FCT.
The fitness function of an individual will produce a unit divided by the average value of the squares of the estimation errors:

here n is the number of steps in the filter modeling process (the number of samples in the recorded signals).

Federated Filter vs. Generalized

I have already described the concept of compiling a generalized FC (OFK) ( Kalman filter - is it difficult? ). I will not dwell in detail. The bottom line is that we compose models in the state space using differential equations of sensors. Next, we need to compose block-diagonal matrices of OFCs by connecting diagonal matrices of sensor models. Those. such matrices

should give us something like the following (for the transition matrix)

where the second index of the coefficients indicates the number of the sensor (measuring channel).
The next important point is the state vector of the FC. Roughly speaking, it contains the displacements of the pendulums of the accelerometers (in the problem statement under consideration) and the speed of their displacement. But we need to calculate what acceleration we need to apply to the block in order to bring the accelerometers to the state calculated using the FC. To do this, we must first calculate the signals of the accelerometers modeled by the Kalman Filter

where

	- block-diagonal matrix composed of sensor model measurement matrices
	- state vector of all sensors in the block

After we received the vector of sensor signals, it remains for us to somehow solve the overdetermined system of eight equations with three unknowns

where

	- guide cosines of the axes of sensitivity of the accelerometers
	- the desired acceleration of the measuring unit

And then suddenly ...

We can only calculate the pseudo-inverse matrix using the Gaussian-Markov least squares method:

where N is the matrix of directional cosines (see above the coefficients n11, n12, ..., n83);
C - matrix of covariance of measurement errors (not to be confused with Coff).
What is bad about the OFK algorithm? Its transition matrix has a lot of zeroes that you don’t want to waste processor time on. It is precisely this problem that the Federal algorithm of the Kalman Filter Type solves. The point is simple. We refuse to compile a single filter that combines all the sensors at once and implement eight separate filters for each of the sensors. Those. instead of one 16th order filter, we use 8 second order filters. Schematically, this can be represented as follows.

The outputs of these eight “private” filters, we, as in the case of the OFC, are substituted into the equation of the Gaussian-Markov least squares and we obtain estimates of the three components of the acceleration of the measuring unit.
What is the disadvantage of Federated Kalman filters? In them, the matrix [1x1] containing the variance of the measurement error of a single sensor is used as the matrix of covariance of measurement errors. Those. instead of the covariance matrix, a scalar equal to the variance of the noise of a specific sensor is used. Thus, noise covariances between the measuring channels are not taken into account. This means that such a filter should be less effective in the case of real noise correlated with each other. This is in theory. In practice, the covariance of noise changes over time, so there is no way to give a guarantee that the OFC will give a “more” optimal estimate than a federal one. It may well turn out that the efficiency of the OFC will be slightly higher or even equal to the effectiveness of the federal filter. This, by the way, remains to be explored.

Synthesis and Simulation Program

Solution Structure

I apologize in advance for Anglicism - I could not find some terms from professional jargon to find the equivalent in "great and mighty." So, I’ll start the story about the program with an enlarged description of the structure of the solution (sorry). Then I will describe the logic of the most interesting methods in my opinion. In the end I’ll talk about how I generated the source data for the program.
The structure of the solution is presented on the mind-map ( flash drive with brains ?) Below. The dashed arrows show the links of the assemblies (references).

Two third-party libraries were used in the program: Math.Net for matrix algebra and random number generation, as well as FileHelpers for loading data from CSV files. In addition, the implemented genetic algorithm engine is based on a third-party implementation of “ A Simple C # Genetic Algorithm"(Barry Lapthorn). True, little is left of the original implementation.
The solution contains one console application project and four projects of the Class Library type containing the main logic.
The GA assembly, as the name suggests, contains an implementation of genetic algorithms. It contains the Specimen structure (an individual data structure) and the GA and SpecimenHelper classes. SpecimenHelper is a static class and contains static methods that simplify working with individual genes (for example, GenerateGenes, Crossover, Mutate, Print). This class also contains instances of ContinuousUniform random number generators from the Math.Net library. This generator had to be used because I found out that the standard Random generator from .Net 4.5 assemblies generates normally distributed random numbers instead of evenly distributed ones. For gene generation, this is quite critical.
The GA class is instance oriented. It is possible to create several instances of the optimizer with different parameters and functions. For example, it is possible in one engine to make the selection of the parameters of the mathematical model of sensors, and in the second one to already select the values ​​of the covariance matrices, while through the closure, “slipping” into the fitness function the currently best assembly of the parameters of the mathematical model.
The Simulation assembly currently contains only one static FedKfSim class. That, in turn, contains the parameters of the federal filter simulation, the ToFedKf extension method for an instance of the Specimen class, which creates a federal filter by the genes of this individual. This class also contains the static Simulate method, in which a federated filter is created for the passed parameters of the individual and the process of simulating the operation of this filter is started.
The Filtering assembly contains implementations of a dynamic model in a state space (class SSF), a private Kalman type filter (class KF), and a federated filter (class FedKF). The SSF class, in addition to instance methods, contains two static methods that allow generating a discrete model of the state space from the coefficients of a continuous transfer function (FS). PF parameters are passed in MatLab notation, i.e.

The KFBuilder static class in the Filtering assembly contains auxiliary methods for generating a state space model and a private FKT, using string records of the coefficients of the numerator and denominator of the continuous FS as source data as well as the required sampling frequency (the inverse of the time quantization period).
The DAL assembly contains the FileParser class, which is used to parse text files containing matrix data, as well as to load signals and noise from CSV files.

Genetic Algorithm Engine

For its operation, it is necessary to set the fitness function (FitnessFunction), population size (PopulationSize), number of generations (GenerationsCount), number of genes in an individual (GenomeSize), probability of crossing (CrossoverRate), probability of mutation (MutationRate).
The Initiation method is designed to generate an initial population of individuals. The method code is presented below (only the most important is left):

The important point here is the use of parallel LINQ. First, an array of indices is created from 0 to the size of the population. For this enumerated instance, a parallel query (.AsParallel ()) is created, to which a select query is already attached, in the body of which an individual instance will be generated and its fitness value will be calculated. At the end, an ordering query (.OrderBy (...)) is attached. These were all requests and this block of code will execute quickly. Values ​​will be updated in the following line:
_currGeneration = newSpecies.ToList(); // Huge load starts here :)
what the comment says. In this case, the calculations will be performed in parallel using the thread pool, therefore, in the body of the select request, it is impossible to put the code for writing values ​​into any shared variable (for example, into a shared array). Such code will greatly slow down the operation of a parallel request (we will still encounter this).
Based on the generated individuals, the fitness table of the individuals is calculated, which will be needed for the “Roulette Wheel” algorithm for selecting individuals for crossing. As you can see from the code, each current (last) value in the table is the sum of the previous value and current fitness. In this way, the table is filled with segments of different lengths - the greater the fitness, the greater the length of the segment (see figure below).

Due to this, it is possible to use “evenly distributed” random variables ranging from zero to the sum of all adaptations to “honestly choose” individuals for crossing so that the most adapted individuals are chosen more often, but “losers” also had a chance to cross. If the random number generator produces normally distributed values ​​(as is the case with Random in .Net 4.5), then individuals will most often be selected from the middle of the fitness table. That's why I wrote above that using ContinuousUniform from the Math.Net package was a critical moment in my case.
The next method I want to talk about is the Selection method.

In this method, selection is made. I did not choose both parents here with the help of a roulette wheel. The first of the parents is selected in order from a little less than half of the fittest individuals. The second parent is already randomly selected. Moreover, if the second parent in genes is close to the first, then the random sampling is repeated until a parent that is sufficiently different in genes is obtained. Then, a crossover procedure is randomly started, which gives out two individuals with mixed genes and one more, whose gene values ​​are selected as the average of the values ​​of the parents' genes.

TODO: Probably, it is necessary to replace the calculation of the average with the calculation of a random number in the range between the values ​​of the parents' genes.

After crossing the new individuals, fitness will have the value double.NaN. The actualization of the fitness values ​​of new individuals is again done in parallel

foreach (var ch in children.AsParallel())
{
    if (double.IsNaN(ch.Fitness))
    {
        var fitness = FitnessFunction(ch);
        var newChild = new Specimen
            {
                //...
            };
        tempGenerationContainer.Add(newChild);
    }
    //...
}

The last GA engine method worth mentioning is the RouletteSelection method.

private int RouletteSelection()
{
    double randomFitness = Rnd.NextDouble() * TotalFitness;
    int idx = -1;
    int first = 0;
    int last = this.PopulationSize - 1;
    int mid = (last - first) / 2;
    while (idx == -1 && first <= last)
    {
        if (randomFitness < _fitnessTable[mid])
        {
            last = mid;
        } 
        else if (randomFitness > _fitnessTable[mid])
        {
            first = mid;
        }
        else if (randomFitness == _fitnessTable[mid])
        {
            return mid;
        }
        mid = (first + last) / 2;
        // lies between i and i+1
        if ((last - first) == 1)
        {
            idx = last;
        }
    }
    return idx;
}

In this method, using the half-division method, a search is made for a segment in the table of fitness where a randomly selected value falls. I repeat, a random number is selected in the range from zero to the sum of all adaptations. The greater the fitness of a particular individual, the greater the likelihood that a randomly selected fitness value will fall into the corresponding segment of the table. In this way, the desired index is pretty fast.

Federated FCT Simulator

As previously written, the federated filter simulator is implemented by the FedKfSim class. The main method in it is the Simulate method.

This method starts an iterative process. At each step, a selection is made of the pure values ​​of the sensor signals, noise, and reference values ​​(block accelerations without noise). Pure signals are summed with noise, and this mixture is fed to the Step method of the federal filter. The evaluation of the block accelerations from the previous step is also presented there, but these values ​​are not used in the current implementation of the private FCT. The Step method of the federated filter produces an array of values ​​- estimates of block accelerations. The difference between them and the reference values ​​will be the current estimation error. At the end of the current step, the average square of the estimation errors is calculated, and the resulting value is added to the sum of the errors. At the end of the iterations, the average error value is calculated for all steps of the process. The output is the number inverse of the average error.

Executable application

The sequence of the application is as follows. First, the console argument is checked. If the argument list is not empty, then an attempt is made to execute one of the valid commands.

switch (cmd)
{
    case "simulate": 
    case "simulation":
        InitializeSimulator();
        FedKfSim.PrintSimResults = true;
        var spec = new Specimen();
        SpecimenHelper.SetGenes(ref spec, ReadSimulationGenes());
        FedKfSim.Simulate(spec);
        break;
    case "set":
        var settingName = args[1];
        var settingValue = args[2];
		var config = ConfigurationManager.OpenExeConfiguration(ConfigurationUserLevel.None);
		config.AppSettings.Settings[settingName].Value = settingValue;
		config.Save(ConfigurationSaveMode.Modified);
		ConfigurationManager.RefreshSection("appSettings");
		Console.WriteLine("'{0}' set to {1}", settingName, settingValue);
        break;
    case "print":
        Console.WriteLine("Current settings:");
        foreach (var name in ConfigurationManager.AppSettings.AllKeys.AsParallel())
        {
            var value = ConfigurationManager.AppSettings[name];
            Console.WriteLine("'{0}' => {1}", name, value);
        }
        break;
	case "help":
	case "?":
	case "-h":
		PrintHelp();
		break;
    default:
		Console.Error.WriteLine(string.Format("\nARGUMENT ERROR\n'{0}' is unknown command!\n", cmd));
		PrintHelp();
		break;
}

The simulate command subtracts the values ​​of the genes from the configuration files for which a federated FCT needs to be generated. The “set” command is intended to change one of the values ​​in the main application configuration file. By the “print” command, the values ​​of all settings from the main configuration file are displayed in the console. The help command prints basic documentation to the console.
When the application starts without arguments, the main parameters of the GA engine are read from the configuration file, as well as the values ​​of the matrices, signals, noise and references from the data files. After loading all the necessary data and parameters, an optimizer instance is created and the evolutionary process is launched.

InitializeSimulator();
var genCount = int.Parse(ConfigurationManager.AppSettings["GenerationsCount"]);
var popSize = int.Parse(ConfigurationManager.AppSettings["PopulationSize"]);
var crossOver = double.Parse(ConfigurationManager.AppSettings["CrossoverRate"], FileParser.NumberFormat);
var mutRate = double.Parse(ConfigurationManager.AppSettings["MutationRate"], FileParser.NumberFormat);
var maxGeneVal = double.Parse(ConfigurationManager.AppSettings["MaxGeneValue"], FileParser.NumberFormat);
var minGeneVal = double.Parse(ConfigurationManager.AppSettings["MinGeneValue"], FileParser.NumberFormat);
var genomeLength = int.Parse(ConfigurationManager.AppSettings["GenomeLength"]);
SpecimenHelper.SimilarityThreshold = double.Parse(
    ConfigurationManager.AppSettings["SimilarityThreshold"], FileParser.NumberFormat);
var ga = new Ga(genomeLength)
{
    FitnessFunction = FedKfSim.Simulate,
    Elitism = true,
    GenerationsCount = genCount,
    PopulationSize = popSize,
    CrossoverRate = crossOver,
    MutationRate = mutRate
};
FedKfSim.PrintSimResults = false;
ga.Go(maxGeneVal, minGeneVal);

Particular attention should be paid to the ReadSignalsAndNoises method.

It uses several separate double variables to store covariance values. As I wrote above, if I used one array, then parallel tasks (Task) would be performed slowly. Actually, this is what I came up with with the initial implementation of this method. I set indices within each task, according to which it was necessary to register the obtained covariance value in a common array. And from this point of view, the code was seemingly safe. But it was carried out extremely slowly. After replacing the array with individual variables, the calculation of the covariance matrix was significantly accelerated.
So what is done in this method? For each unique combination of two rows of noise, a separate asynchronous task is created. In these problems, the mutual variation of two random processes is calculated.

private static double CalcVariance(IEnumerable v1, double mean1, IEnumerable v2, double mean2, int length)
{
    var zipped = v1.Take(length).Zip(v2.Take(length), (i1, i2) => new[] { i1, i2 });
    var sum = zipped.AsParallel().Sum(z => (z[0] - mean1) * (z[1] - mean2));
    return sum / (length - 1);
}

The formula is similar to the dispersion calculation formula. Only instead of the square of the deviations from the average of one random series, is the product of deviations from the average values ​​of two separate series used?
Initial data
The initial data for the program are numerical series with noise, sensor signals and true values ​​of the block acceleration components, as well as filter parameters: the matrix of direction cosines (taken from my graduation project, corresponds to the configuration of the redundant block described in one of my previous articles), matrix of covariances of process noise (empirically defined).
The reference values ​​of the block accelerations were set as a combination of two sinusoids. These values ​​through the matrix of guide cosines of the block were converted into sensor signals. Sensor noise was generated in a hybrid way. I took a recording of the signal of a real sensor, which was at rest during the recording. Having removed the constant component from this signal, I got the sensor noise in statics (this is enough for debugging the algorithms). By the way, these noises were very close to white with a normal distribution. I cut these noises into short samples of 1000 values, scaled their intensities to bring the variances to the range [0.05..0.15]. Thus, we can say that I used real noise in my research.

results

The generated federal filter reduced the average noise variance by almost half (i.e., it reduced the root-mean-square error by about 1.38 times). One could achieve an even more significant reduction in the variance of errors by increasing inertia, but then the dynamic delay would increase. In my opinion, a fully operational noise filter was generated using the GA.

Conclusion

I don’t have much free time, therefore, when implementing the main functional, there wasn’t time for perfectionism. In some places, there are gaps in the security of the code (mainly regarding the validation of arguments); in some places, the general recommendations of the “C # Code Style Guide” are not followed. Surely there are code blocks with suboptimal performance. But despite all its shortcomings, it works quite well. I tested it on Intel Core i3 CPUs and the optimization process was pretty fast. In addition, due to the use of TPL, processor resources were used quite efficiently. The implemented algorithms consumed almost all of the processor’s free time, but thanks to the normal priority of asynchronous TPL tasks, the rest of the user programs worked without serious locks. I ran the implemented program in the console and switched to work in Visual Studio 2012 with the built-in resolver, periodically switched to Outlook and MS Word, debugged the working project in parallel in IE and Chrome browsers, without any serious brakes. In general, it was a good experience using the TPL asynchronous programming library, a good result was immediately obtained without any serious difficulties.
Thanks for attention! I hope the article and code will be useful to you.

Github sources : github.com/homoluden/fedkf-ga