\chapter{Deep Reinforcement Learning and Kalman Filtering Algorithms}

% % use [][] to prepend/postpone text to the citation
% \cite[Hi][Goofy]{IEEEexample:article_typical}

% \si{\kilo\gram\per\second}

% % generic figure
% \begin{figure}[h]
% \centering
% \includegraphics[width=.9\linewidth]{images/logo/logoPoliTo_with_name_wrong.png}
% \caption{Hi}

% \end{figure}

\section{Deep Reinforcement Learning}
\subsection{Introduction to Deep Reinforcement Learning} %Section - 1.1
The difference between deep learning and reinforcement learning is that deep learning is learning from a training set and then applying that learning to a new data set, while reinforcement learning is dynamic learning by adjusting actions based on feedback to maximize a reward. Reinforcement learning is dynamic learning with a trial-and-error method to maximize the outcome, while deep reinforcement learning is learning from existing knowledge and applying it to a new data set.
    \subsubsection{Feedback in Reinforcement Learning}
    Deep Reinforcement Learning is a complex sequential decision-making problem under uncertainty.
    The kinds of feedback in DRL are as follows:
        \begin{itemize}
            \item \textbf{Sequential:} The opposite of sequential feedback is one-shot feedback. Decisions don't have long-term consequences in         problems that deal with one-shot feedback, such as supervised learning. One of the main challenges of sequential feedback is that your agents can receive delayed information, like in a chess game. Delayed feedback makes it tricky to interpret the source of the feedback.
            \item \textbf{Evaluative:} The crux of evaluative feedback is that the goodness of the feedback is only relative because the environment is uncertain. While not having access to the model of the environment, we must explore to gather new information or improve on our current information. Eventually, the exploration-exploitation trade-off arises. The opposite of evaluative feedback is supervised feedback. In a classification problem, your model receives supervision; during learning, your model is given the correct labels for each of the samples provided.
            \item \textbf{Sampled}: in Deep Reinforcement Learning, agents need to generalize using the gathered feedback and make intelligent decisions based on that generalization. The opposite of sampled feedback is exhaustive feedback. To exhaustively sample environments means agents have access to all possible samples.
        \end{itemize}
    \subsubsection{Function Approximation in Reinforcement Learning}
        Motivations for the use of function approximation to solve reinforcement learning problems
            \begin{itemize}
                \item \textbf{High Dimensionality of State and Action Space}: The main drawback of tabular reinforcement learning is that using a table to represent value functions is no longer practical in complex problems. Environments can have high-dimensional state spaces, meaning that the number of variables that comprise a single state is vast.
                \item \textbf{Continuous State and Action Space}: Environments can additionally have continuous variables, meaning that a variable can take on an infinite number of values. To clarify, state and action spaces can be high dimensional with discrete variables, they can be low dimensional with continuous variables, and so on.
            \end{itemize}

        Advantages when using function approximation
            Function approximation can make our algorithms more efficient and more complex relationships can be discovered with a non-linear function approximator, such as a neural network.

    \subsubsection{Value Functions}
    Using neural networks to approximate value functions can be done in many ways. There are many different value functions we could approximate, as:
        \begin{itemize}
            \item The state-value function $v(s)$: It helps you know how much expected total discounted reward you can obtain from state s and using policy $\pi$ thereafter. But, to determine which action to take with a V-function, you also need the MDP of the environment so that you can do a one-step look-ahead and take into account all possible next states after selecting each action.
            
            \item The action-value function $q(s,a)$: If we had the values of state-action pairs, we could differentiate the actions that would lead us to either gain information, in the case of an exploratory action, or maximize the expected return, in the case of a greedy action. Therefore, it can be used in control problems.
            \item The action-advantage function $a(s,a)$: it helps us differentiate between values of different actions and lets us easily see how much better than average an action is.
        \end{itemize}

    \subsubsection{Neural Network Architecture}
        We can have two different Neural Network Architecture, and the choice depends on the complexity of the state, action and environment in which the agent is training.
            \begin{itemize}
                \item State-action-in-value-out architecture: This architecture is designed to take both the state and action as inputs and output a value, which usually represents the expected reward or the quality of the action given the state. It's ideal for situations where the relationship between state-action pairs and the corresponding value is crucial to be explicitly modeled.
                \item State-in-values-out architecture: Suited for scenarios where the action space is vast or continuous, this architecture processes the state as its input and returns a value for each potential action or a distribution over continuous actions. This design proves invaluable in environments where evaluating every individual state-action pair would be computationally impractical due to the massive number of possible actions.
            \end{itemize}
            
            

    \subsubsection{Objective Function for Optimization}
        \begin{align}
            L_i(\theta_i) = \mathbb{E}_{s,a}[(q^*(s,a) - Q(s,a;\theta_i))^2]
            \label{eq:objective_finction}
        \end{align}
        The equation \ref{eq:objective_finction} reports an ideal objective in value-based deep reinforcement learning would be to minimize the loss with respect to the optimal action-value function $q^*$. We want to have an estimate of $q^*$, $Q$, that tracks exactly that optimal function.
        If we had access to the optimal action-value function, we’d use that, but if we had access to sampling the optimal action-value function, we could then minimize the loss between the approximate and optimal action-value functions. Furthermore, the optimal action-value function would be as follows:
        \begin{align}
            q^*(s,a) = \max_{\pi}\mathbb{E}_{\pi}[G_t|S_t = s, A_t = a], \forall{s} \in {S} , \forall{a} \in {A(s)}
        \end{align}
        Based on the above function, an optimal action-value function is a policy that gives the maximum expected return for each action in each state. Since we do not have the optimal policy, we can not sample the optimal Q-values, and as a result, we do not have the optimal action-value function $q_*$. Instead, we must alternate between evaluating a policy (by sampling actions from it), and improving it (using an exploration strategy, such as epsilon-greedy)

    \subsubsection{Targets for Policy Evaluation}
    We can use different targets to estimate the action-value function of a policy $\pi$.
        \begin{itemize}
            \item {Monte Carlo targets (MC)}: Use all rewards found in a trajectory from a start state to the terminal state.
            \item {Temporal Difference targets (TD)}: Use the value of the next state as an estimate of all rewards to go.
            \item {N-step targets}: $N-step$ is like TD, but instead of bootstrapping after one step, you use $n$ steps.
            \item {lambda targets}: Lambda target mixes in an exponentially decaying fashion all $n-step$ targets into one.
        \end{itemize}

    \subsubsection{Learning Strategy}
        \begin{itemize}
            \item {On-Policy}: if we were to use the on-policy target, the target would approximate the behavioral policy; the policy-generating behavior and the policy being learned would be the same.
            \item {Off-Policy}: In off-policy target we always approximate the greedy policy, even if the policy-generating behavior isn’t totally greedy.
        \end{itemize}
        Notice that both on-policy and off-policy targets estimate an action-value function.
        \textbf{The Q-learning target as an off-policy TD target}
        In practice, an online Q-learning target would look something like the following:
        \begin{align}
            y^{Q-learning}_{i} = R_{t+1} + \gamma \max_{a} Q(S_{t+1}, a;\theta_{i})
        \end{align}
        The bottom line is we use the experienced reward and the next state to form the target.
        We can plug in a more general form of this Q-learning target as in the following:
        \begin{align}
            L_i(\theta_{i})= \mathbb{E}_{s,a,r,s^{'}}[(r + \gamma \max_{a} Q(s^{'},a^{'};\theta_{i})-Q(s,a;\theta_{i}))^2]
        \end{align}
        But it’s basically the same. We’re using the expectation of experience tuples to minimize the loss. When differentiating through this equation, you must notice the gradient doesn’t involve the target. The gradient must only go through the predicted value.
        \begin{align}
            \nabla _{\theta_{i}}L_i(\theta_{i})= \mathbb{E}_{s,a,r,s^{'}}[(r + \gamma \max_{a} Q(s^{'},a^{'};\theta_{i})-Q(s,a;\theta_{i})) \nabla _{\theta_{i}} Q(s, a; \theta _{i})]
        \end{align}

    \subsubsection{Optimization Method}
    Gradient descent is a stable optimization method given a couple of assumptions: data must be independent and identically distributed (IID), and targets must be stationary. In reinforcement learning, however, we cannot ensure any of these assumptions hold, so choosing a robust optimization method to minimize the loss function can often make the difference between convergence and divergence. Some optimization methods are introduced in the following:

    \begin{itemize}
        \item \textbf{Gradient Decent}\\
        Based on the dataset which is given for the optimization, the gradient descent algorithm can be defined as follows:
        \begin{itemize}
            \item Batch Gradient Decent: The batch gradient descent algorithm takes the entire dataset at once, calculates the gradient of the given dataset, and steps toward this gradient a little bit at a time. Then, it repeats this cycle until convergence. When you have a considerable dataset with millions of samples, batch gradient descent is too slow to be practical. Moreover, we don’t even have a dataset in reinforcement learning in advance, so batch gradient descent isn’t a practical method for our purpose either. Batch gradient descent goes smoothly toward the target because it uses the entire dataset at once, so a lower variance is expected.
            \item Mini Batch Gradient Decent: In mini-batch gradient descent, we use only a fraction of the data at a time. We process a mini-batch of samples to find its loss, then backpropagate to compute the gradient of this loss, and then adjust the weights of the network to make the network better at predicting the values of that mini-batch. With mini-batch gradient descent, you can control the size of the mini-batches, which allows the processing of large datasets. In mini-batch gradient descent we use a uniformly sampled mini-batch. This results in noisier updates, but also faster processing of the data.
            \item Stochastic Gradient Decent \cite{SGD, SDG2}: At the other extreme, you can set the mini-batch size to a single sample per step. In this case, you’re using an algorithm called stochastic gradient descent. With stochastic gradient descent, in every iteration we step through only one sample. This makes it a noisy algorithm. It wouldn’t be surprising to see several steps taking us further away from the target, and later back toward the target.
        \end{itemize}
        
        	\begin{figure}[h]
        		\centering
        		\includegraphics[width=.9\linewidth]{images/neuralnetworks/BGD-BGD-SGD.png}
        		\caption{Batch Gradient Decent, Mini Batch Gradient Decent, Stochastic Gradient Decent}

        	\end{figure}
        \item \textbf{Gradient Decent with Momentum}\\
        An improved gradient descent algorithm is called gradient descent with momentum, or momentum for short \cite{GDM}. This method is a mini-batch gradient descent algorithm that updates the network’s weights in the direction of the moving average of the gradients, instead of the gradient itself.
        
        \begin{figure}[h]
        	\centering
        	\includegraphics[width=.9\linewidth]{images/neuralnetworks/GD-GDM.png}
        	\caption{Gradient Decent Vs Gradient Decent with Momentum}

        \end{figure}
        
        \item \textbf{Root Mean Square Propagation (RMSprop)}\\
        Root mean square propagation (RMSprop) is an alternative to using momentum. RMSprop and momentum do the same thing of dampening the oscillations and moving more directly toward the goal, but they do so differently. While momentum takes steps in the direction of the moving average of the gradients, RMSprop takes the safer bet of scaling the gradient in proportion to a moving average of the magnitude of gradients. It reduces oscillations by merely scaling the gradient in proportion to the square root of the moving average of the square of the gradients or, more simply put, in proportion to the average magnitude of recent gradients.
        \item \textbf{Adaptive Moment Estimation (Adam) \cite{Adam}}\\
        Adam is a combination of RMSprop and momentum. The Adam method steps in the direction of the velocity of the gradients, as in momentum. But, it scales updates in proportion to the moving average of the magnitude of the gradients, as in RMSprop. These properties make Adam as an optimization method a bit more aggressive than RMSprop, yet not as aggressive as momentum.
    \end{itemize}



\subsection{Value-Based Deep Reinforcement Learning Algorithms}
    This section will introduce some value-based algorithms based on their improvements.
    \subsubsection{Deep Q-Network}
        Deep Q-Network (DQN) is an algorithm for training a deep neural network to play a game or control a system through reinforcement learning. It was introduced by Google DeepMind in a 2015 paper titled "Human-level control through deep reinforcement learning" \cite{DQN, DQN2}.\\
        The DQN algorithm combines Q-learning, a standard reinforcement learning algorithm, with a deep neural network as a function approximator. Q-learning is a model-free algorithm that allows an agent to learn the optimal action to take in a given state by updating an estimate of the maximum expected future reward for each action. The neural network is used to approximate the Q-function, which gives the expected future reward for each action in a given state.\\
        The DQN algorithm uses experience replay, where the agent stores a history of its experiences in a buffer and samples from this buffer to update the network weights \cite{EXPERIENCE_REPLAY}. This helps to decorrelate the experiences and stabilize the learning process. Experience replay consists of a data structure, often referred to as a replay buffer or a replay memory, that holds experience samples for several steps, allowing the sampling of mini-batches from a broad set of past experiences. Having a replay buffer allows the agent two critical things. First, the training process can use a more diverse mini-batch for performing updates. Second, the agent no longer has to fit the model to the same small mini-batch for multiple iterations. The algorithm also uses a target network, which is a copy of the primary network that is used to compute the target Q-values for the primary network's updates. This helps to reduce the variance in the updates and improve the stability of the learning process.
        
        \begin{align}
        	\begin{split}
        		\nabla _{\theta_{i}}L_i(\theta_{i}) &= \mathbb{E}_{(s,a,r,s^{'})\sim u(D)} \left[(r + \gamma \max_{a} Q(s^{'},a^{'};\theta^{-}) \right. \\
        		&\quad \left. -Q(s,a;\theta_{i})) \nabla _{\theta_{i}} Q(s, a; \theta _{i}) \right]
        	\end{split}
        \end{align}
        
        
        A target network is a previous instance of the neural network that we freeze for a number of steps. The gradient update now has time to catch up to the target, which is much more stable when frozen. This adds stability to the updates. we’re now obtaining the experiences we use for training by sampling uniformly at random the replay buffer D, instead of using the online experiences. \\
        It’s important to note that in practice, we don’t have two “networks,” but instead, we have two instances of the neural network weights. We use the same model architecture and frequently update the weights of the target network to match the weights of the online network, which is the network we optimize at every step. Using target networks prevents the training process from spiraling around because we’re fixing the targets for multiple time steps, thus allowing the online network weights to move consistently toward the targets before an update changes the optimization problem and a new one is set. By using target networks, we stabilize training, but we also slow down learning because you’re no longer training on up-to-date values. \\
        Another way you can lessen the non-stationarity issue, to some degree, is to use larger networks. More powerful networks make subtle differences between states more likely to be detected. Larger networks reduce the aliasing of state-action pairs; the more powerful the network, the lower the aliasing; the lower the aliasing, the less apparent correlation between consecutive samples. And all of this can make target values and current estimates look more independent of each other.\\
        But, a more powerful neural network takes longer to train. It needs not only more data (interaction time) but also more compute (processing time). Using a target network is a more robust approach to mitigating the non-stationary problem

    \subsubsection{Double DQN}
        Q-learning tends to overestimate action-value functions. Our DQN agent is no different; we’re using the same off-policy TD target, after all, with that max operator. The crux of the problem is simple: We’re taking the max of estimated values. Estimated values are often off-center, some higher than the true values, some lower, but the bottom line is that they’re off. The problem is that we’re always taking the max of these values, so we have a preference for higher values, even if they aren’t correct. Our algorithms show a positive bias, and performance suffers.
        
		\begin{align}
			\begin{split}
				\nabla _{\theta_{i}}L_i(\theta_{i}) &= \mathbb{E}_{(s,a,r,s^{'})\sim u(D)} [ (r + \gamma Q(s^{'},\arg \max_{a^{'}}Q(s^{'},a^{'};\theta^{-});\theta^{-}) \\
				&\quad - Q(s,a;\theta_{i})) \nabla _{\theta_{i}} Q(s, a; \theta _{i}) ]
			\end{split}
		\end{align}

        Double Deep Q-Network (Double DQN) is an extension of the Deep Q-Network (DQN) algorithm for training deep neural networks to play games or control systems through reinforcement learning. It was introduced by Google DeepMind in a 2015 paper titled "Deep Reinforcement Learning with Double Q-learning" \cite{DDQN}. \\
        In the DQN algorithm, the Q-values for each action in a given state are approximated using a deep neural network. The Q-values are updated using the Bellman equation, which is a recursive relationship that describes the relationship between the expected future reward for an action and the immediate reward and the expected future reward for the next action. In the Bellman equation, the expected future reward for the next action is typically computed using the same neural network that is used to approximate the Q-values. \\
        However, this can lead to an overestimation of the expected future reward, which can cause the Q-values to be biased and the learning process to be unstable. The Double DQN algorithm addresses this issue by using two separate neural networks to approximate the Q-values and to compute the expected future reward for the next action. The primary network is used to select the action, and the target network is used to compute the expected future reward. This helps to reduce the overestimation of the expected future reward and improve the stability of the learning process. \\
        Like the DQN algorithm, the Double DQN algorithm also uses experience replay and a target network to stabilize the learning process. It has been successfully applied to a variety of tasks and has been shown to improve the performance of the DQN algorithm in some cases.
        
    \subsubsection{Dueling DDQN}
        Dueling Double Deep Q-Network (Dueling Double DQN) is an extension of the Double Deep Q-Network (Double DQN) algorithm for training deep neural networks to play games or control systems through reinforcement learning. It was introduced by Google DeepMind in a 2016 paper titled "Dueling Network Architectures for Deep Reinforcement Learning". \cite{DuellingDDQN}. \\
        Like the Double DQN algorithm, the Dueling Double DQN algorithm uses two neural networks: a primary network and a target network. The primary network is used to approximate the Q-values for each action in a given state and to select the action to take. The target network is used to compute the expected future reward for the next action. \\
        The Dueling Double DQN algorithm introduces a new architecture for the primary network that separates the network into two streams: one for the state value function and one for the action advantage function. The state value function gives the expected return for a given state, and the action advantage function gives the advantage of taking a particular action over the average of all actions in a given state. The Q-values are then computed as the sum of the state value function and the action advantage function for each action. \\
        This architecture has several benefits. First, it allows the network to learn the relative importance of the state value function and the action advantage function separately, which can improve the learning process. Second, it allows the network to learn the optimal action to take in a given state without having to learn the Q-values for all actions in that state, which can reduce the complexity of the learning process. Finally, it allows the network to generalize better to new states and actions, which can improve the performance of the algorithm. \\
        Like the Double DQN algorithm, the Dueling Double DQN algorithm also uses experience replay and a target network to stabilize the learning process. It has been successfully applied to various tasks and has been shown to improve the performance of the Double DQN algorithm in some cases.
		
		\begin{align}
			Q_(s,a;\theta, \alpha, \beta) &= \left[ V(s;\theta, \beta) \right] + \left[ A(s,a;\theta,\alpha) \right]
		\end{align}
		
		The Q-function is parameterized by theta, alpha, and beta. Theta represents the weights
		of the shared layers, alpha the weights of the action-advantage function stream, and beta
		the weights of the state-value function stream.
		
		\begin{align}
			Q_(s,a;\theta, \alpha, \beta) &= \left[ V(s;\theta, \beta) \right] + \left( \left[ A(s,a;\theta,\alpha) \right] - \frac{1}{|\textit{A}|} \sum_{a^{'}} A(s,a^{'};\theta,a) \right)
		\end{align}
		
		But because we cannot uniquely recover the Q from V and A , we use the above
		equation in practice. This removes one degree of freedom from the Q-function. The
		action-advantage and state-value functions lose their true meaning by doing this. But in
		practice, they’re off-centered by a constant and are now more stable when optimizing.
		
		
    \subsubsection{Prioritized Experience Replay Buffer (PER)}
        Prioritized Experience Replay (PER) is a technique for improving the efficiency of the experience replay process in reinforcement learning. It was introduced by Google DeepMind in a 2016 paper titled "Prioritized Experience Replay" \cite{PER}. \\
        In reinforcement learning, experience replay is a technique where the agent stores a history of its experiences in a buffer and samples from this buffer to update the network weights. This helps to decorrelate the experiences and stabilize the learning process. However, traditional experience replay uniformly samples from the buffer, which can be inefficient because it may sample trivial or unimportant experiences more frequently than important or rare experiences. \\
        To address this issue, the PER algorithm introduces a priority value for each experience in the buffer. The priority value reflects the importance of the experience, with higher values indicating more important experiences. The algorithm then samples from the buffer using a distribution that is proportional to the priority values, which allows it to sample more important experiences more frequently. \\
        The priority values are updated using the temporal difference error, which measures the difference between the expected and actual return for an action. Experiences with larger temporal difference errors are assigned higher priority values because they are more likely to be important for learning. \\
        The PER algorithm has been shown to improve the efficiency of the learning process by allowing the agent to learn more from important experiences and to learn more quickly. It has been used in combination with a variety of reinforcement learning algorithms, including Deep Q-Networks (DQN) and Double Deep Q-Networks (Double DQN).
        
        \begin{align}
        	|\delta_{i}| = \underbrace{ | \underbrace{ \underbrace{ r + \gamma Q\left(s^{'},\arg\max_{a^{'}}Q(s^{'},a^{'};\theta_{i}, \alpha_{i}, \beta_{i}\right); \theta^{-}, \alpha^{-}, \beta^{-} }_{\text{Duelling DDQN target}} - Q(s, a; \theta_{i}, \alpha_{i}, \beta_{i}) }_{\text{Duelling DDQN error}} | }_{\text{Absolute Duelling DDQN error}}
        \end{align}
        
        	

\subsection{Policy-Based Deep Reinforcement Learning Algorithms}

    Pure policy-gradient methods constitute a class of reinforcement learning algorithms that focus on directly optimizing stochastic policies, eschewing the use of value functions. Unlike other methods such as Q-learning or Actor-Critic, which leverage value functions as a means of estimating the expected cumulative reward from a given state or state-action pair, these methods directly optimize the policy to guide the agent's interactions with the environment. Stochastic policies, central to pure policy-gradient methods, define a probability distribution over actions for each state, promoting exploration in the action space and enabling the agent to discover superior actions over time. As a result, pure policy-gradient methods offer a direct and efficient approach to learning optimal strategies for agents interacting with complex environments, bypassing the need for intermediate value estimations \cite{PolicyBased}.
    
    While accurately determining the values of states in reinforcement learning can be a complex task, employing a rough approximation can prove beneficial for reducing the variance of the policy-gradient objective. By leveraging approximate state values, one can effectively mitigate the fluctuations in the policy-gradient estimates, thereby enhancing the stability and convergence of the learning process. Although pure policy-gradient methods typically focus on direct policy optimization without utilizing value functions, incorporating approximate state values can help strike a balance between exploration and exploitation in the action space, ultimately leading to more efficient and robust policy learning in challenging environments.
    
    Incorporating a value function as a baseline or for calculating advantages can significantly reduce the variance of the targets utilized in policy-gradient updates, ultimately facilitating more efficient and stable learning. By employing a value function, the algorithm can differentiate between the intrinsic value of a state and the advantage of taking specific actions, which allows for more precise policy updates. Consequently, this reduction in variance often translates to accelerated learning, as the agent can converge more rapidly to an optimal policy. Thus, while pure policy-gradient methods focus on direct policy optimization, integrating value functions in the learning process can enhance the overall performance and robustness of the reinforcement learning algorithm.
    
    In addressing continuous environments, researchers often investigate methods that learn deterministic policies \cite{pmlr-v32-silver14}, which are characterized by their consistency in returning the same action, deemed optimal, when presented with an identical state. Deterministic policies offer a distinct advantage in continuous environments, as they facilitate more efficient exploration of the action space and provide a clear action selection mechanism. By focusing on the optimal action for each state, these methods aim to learn a concise and effective policy that guides the agent's interactions with the environment, enabling it to navigate complex and continuous scenarios with improved performance and reliability.
    
    \subsubsection{Deep Deterministic Policy Gradient (DDPG)}
        In this section, the focus is on the Deep Deterministic Policy Gradient $(DDPG)$ algorithm, an approach that can be viewed as an extension of DQN for continuous action spaces. DDPG incorporates key techniques found in DQN, such as using a replay buffer for off-policy training of an action-value function and employing target networks for training stabilization. However, DDPG goes beyond DQN by training an approximate deterministic policy that represents the optimal action. As a result, DDPG is classified as a deterministic policy-gradient method specifically designed for continuous action spaces, offering an efficient and robust solution for tackling complex environments that demand continuous decision-making \cite{DDPG}.

        DDPG can be visualized as an algorithm with a similar architecture to DQN, sharing some key aspects in the training process. Both DDPG and DQN involve the agent collecting experiences online and storing them in a replay buffer. During each step, the agent retrieves a mini-batch from the buffer, typically sampled uniformly at random, and uses it to calculate a bootstrapped Temporal Difference (TD) target and train a Q-function.

        However, there are notable differences between DQN and DDPG. While DQN determines the greedy action using an $argmax$ operation on the target Q-function, DDPG employs a target deterministic policy function trained to approximate the greedy action. This means that, in DDPG, the best action in the next state is directly approximated using a policy function, rather than relying on the $argmax$ of the Q-function as in DQN.

        The use of a deterministic policy function in DDPG has several advantages:
        \begin{itemize}
            \item It bypasses the need for an $argmax$ operation, which can be computationally expensive in continuous action spaces.
            \item The policy function provides a more direct approximation of the optimal action, potentially leading to faster convergence.
            \item The deterministic policy function can help reduce the exploration-exploitation trade-off, as it directly targets the optimal action for each state.
        \end{itemize}

        This approach can be advantageous in continuous action spaces, as it eliminates the need for $argmax$ calculations and offers more direct action approximations, potentially leading to improved learning efficiency and performance.

        The DDPG value function objective is as follows:

		\begin{align}
			\begin{split}
				L_i(\theta_{i}) &= \mathbb{E}_{(s_{KF},a,r,s^{'}_{KF})\sim u(D)} \left[ \left( r + \gamma Q\left(s^{'}_{KF},\mu (s^{'}_{KF};\phi^{'});\theta^{-}\right) \right. \right. \\
				&\quad \left. \left. - Q\left(s_{KF},a;\theta_{i}\right) \right)^{2} \right]
			\end{split}
		\end{align}


        It means that in DDPG, similar to DQN, a mini-batch is sampled from the replay buffer during the training process. However, DDPG deviates from DQN by learning a deterministic policy, denoted as mu, instead of relying on the $argmax$ operation over the Q-function. The policy mu is trained to approximate the deterministic greedy action for the given state, thereby streamlining the action selection process and improving the efficiency of learning in continuous action spaces.

        To implement DDPG effectively, the introduction of a policy network is necessary. The goal of this network is to provide the optimal action for a given state, and it must be differentiable with respect to the action. This requirement makes it essential for the action to be continuous, ensuring efficient gradient-based learning. The optimization objective is straightforward, as it involves using the expected Q-value derived from the policy network, mu. The agent's task is to identify the action that maximizes this value. It is important to note that, in practice, optimization techniques usually focus on minimizing the negative of this objective, as most optimization algorithms are designed for minimization problems.

        Therefore, the DDPG’s deterministic policy objective:

        \begin{align}
            J_i(\phi_{i})= \mathbb{E}_{(s_{KF})\sim u(D)}\left[Q(s_{KF},\mu (s_{KF};\phi);\theta)\right]
        \end{align}

        In our case, target networks are not used, and instead, the online networks are employed for both the policy (action selection) and the value function (action evaluation). This approach simplifies the architecture and training process. Moreover, since a mini-batch of states is sampled for training the value function, these same states can be efficiently utilized for training the policy network as well. This simultaneous use of state samples for both networks ensures consistency in learning and promotes computational efficiency during the training process.

        In DDPG, deterministic greedy policies are trained with the goal of taking in a state and returning the optimal action for that state. However, in an untrained policy, the actions generated might not be accurate, even though they are deterministic. As previously discussed, agents need to strike a balance between exploiting existing knowledge and exploring new possibilities. Due to the deterministic nature of the DDPG agent's policy, on-policy exploration is not feasible, as it would result in the agent being "stubborn" and consistently selecting the same actions.

        To address this issue, DDPG employs off-policy exploration, injecting Gaussian noise into the actions selected by the policy. This allows the agent to explore new actions and learn from different experiences, which may be crucial for the agent's overall performance. Consequently, in DDPG, the agent explores by adding external noise to the actions, utilizing off-policy exploration strategies to efficiently learn and adapt to its environment.

    \subsubsection{Twin-Delayed Deep Deterministic Policy Gradient Algorithm (TD3)}
    
    This algorithm is introduced by Scott Fujimoto et al. in 2018 \cite{TD3}. There are some differences between TD3 and DDPG algorithms which can be considered as the improvements to the DDPG algorithm. These differences are as follows:

    
    \begin{itemize}
    	\item It has double learning technique as DDQN instead of just one learning network:\\
    	
    	In TD3, a particular kind of Q-function with two separate networks is used which leads to two different estimations of the state-action pairs.
    	
    	\begin{align}
    		J_{i}(\theta_{i}^{a}) = \mathbb{E}_{(s_{KF}, a, r, s_{KF}^{'})\sim u(D)}\left[ (\textit{TWIN}^{target} - Q(s,a;\theta_{i}^{a}))^2 \right]
    	\end{align}
    	
    	\begin{align}
    		J_{i}(\theta_{i}^{b}) = \mathbb{E}_{(s_{KF}, a, r, s_{KF}^{'})\sim u(D)}\left[ (\textit{TWIN}^{target} - Q(s,a;\theta_{i}^{b}))^2 \right]
    	\end{align}
    	
    	The twin network loss would be the some of Mean Square Errors of each networks.
    	
    	\begin{align}
    		\textit{TWIN}^{target} = r + \gamma \min _ {n} Q\left(s_{KF}^{'}, \mu(s^{'}, \phi^{-});\theta^{n,-}\right)
    	\end{align}
    	
    	And the target is calculated as the minimum between the two networks. Also, consider the target network is used for both the policy and value networks    	
    	
    	\item It adds noise to both the action passed to the environment and to the target actions which leads to more robustness to approximation error:\\
    	
    	To improve the exploration through the environment, a Gaussian noise is injected to actions used for the environment as well as to the actions used to calculate the targets. By this technique, the network would be more generalized over similar actions. 
    	
    	$$clamp(x,l,h) = max(min(x,h),l)$$
    	
    	This is the definition of the clamp function which clips a value $x$ between a low $l$ and a high $h$.
    	
    	So, we have:
    	
    	\begin{align}
    		a^{',smooth} = clamp(\mu(s^{'}, \phi^{-})+clamp(\epsilon, \epsilon-l, \epsilon-h), a-l, a-h)
    	\end{align}
    	
    	The clipped Gaussian noise $\epsilon$ is introduced and added to the action to smooth that. Firstly, the $\epsilon$ is sampled and then clamped to be between a preset min and max for $\epsilon$. Then, the clipped Gaussian noise is added to the action and the action is clamped to be between the min and max allowable action according to the specific environment. The smoothed action is used to calculate the $TD3^{target}$ as follows:
    	
    	\begin{align}
    		\textit{TD3}^{target} = r+ \gamma \min _ {n} Q(s^{'}, a^{',smooth};\theta^{n,-})
    	\end{align}
    	
    	\item It delays updates to the policy and target networks which leads the online Q-function updates more frequently:\\
    	
    	This delay is advantageous when the online Q-function undergoes abrupt changes early in the training. By pausing the policy's updates for several value function updates, it enables the value function to stabilize and produce more accurate values before influencing the policy. The suggested delay for the policy and target networks is every other update to the online Q-function.
    	
    	The other thing that you may notice in the policy updates is that we must use one of the networks of the online value model for getting the estimated Q-value for the action coming from the policy. In TD3, we use one of the two networks, but the same network every time.
    	
    \end{itemize}
    
     
    
    \subsubsection{Soft Actor-Critic Algorithm (SAC)}

    Off-policy refers to the learning approach in which the algorithm leverages experiences generated by a behavior policy that is distinct from the policy being optimized. In the context of DDPG and TD3, this is achieved by employing a replay buffer. A replay buffer is a memory structure that stores experiences (comprising state, action, reward, and next state) gathered from multiple previous policies. The replay buffer enables the algorithms to learn from a diverse set of experiences, which can lead to more stable and efficient learning \cite{SAC}.

    To facilitate exploration, both DDPG and TD3 incorporate off-policy exploration strategies. In particular, they use Gaussian noise injection. Gaussian noise is a type of random noise that follows a Gaussian distribution. By adding this noise to the action vectors that interact with the environment, the algorithms encourage the exploration of various states and actions. This strategy helps the agent to discover novel and potentially better solutions, as opposed to merely exploiting the current knowledge.

    In summary, DDPG and TD3 are off-policy reinforcement learning algorithms that train deterministic policies. They make use of a replay buffer to store and learn from diverse experiences generated by different behavior policies. To promote exploration, they apply Gaussian noise injection to the action vectors that are sent to the environment. These techniques contribute to the effectiveness and stability of the learning process.

    The on-policy agents utilize stochastic policies, which inherently incorporate a degree of randomness. This randomness plays a crucial role in exploration, as it allows the agents to discover new possibilities and experiences.

    In order to further encourage the element of randomness within these stochastic policies, an additional component called the entropy term is introduced into the loss function. By incorporating this entropy term, we can ensure that the agents maintain a healthy balance between exploration and exploitation, which is essential for effective learning and decision-making. This approach helps prevent the agents from becoming overly focused on a single strategy or getting stuck in a local optimum, thus promoting more diverse and well-rounded learning experiences.

    In this section, we delve into an algorithm known as the Soft Actor-Critic (SAC), which uniquely combines elements from two distinct paradigms. The SAC algorithm shares similarities with off-policy methods like DDPG (Deep Deterministic Policy Gradient) and TD3 (Twin Delayed Deep Deterministic policy gradient), as it operates in an off-policy manner. However, unlike these algorithms that utilize deterministic policies, SAC employs a stochastic policy.

    By integrating the best of both worlds, the Soft Actor-Critic algorithm is able to reap the benefits of off-policy learning, such as efficient and stable updates from a replay buffer, while also harnessing the exploration advantages provided by stochastic policies. This fusion results in an algorithm that can achieve robust and effective learning, while maintaining a strong ability to explore and adapt to a wide range of environments and situations.

    Incorporating entropy into the Bellman equations is a key aspect of the Soft Actor-Critic algorithm. This unique feature involves integrating the entropy of the stochastic policy directly into the value function, which the agent aims to maximize. The simultaneous maximization of both expected total reward and expected total entropy inherently promotes diverse behaviors while ensuring the agent continues to maximize the expected return.

    By factoring in the entropy, the SAC algorithm encourages the agent to explore a wide range of possible actions and strategies, rather than simply focusing on a narrow set of options. This approach fosters greater adaptability and learning, as the agent is exposed to a broader spectrum of experiences and can fine-tune its decision-making process accordingly. As a result, the agent's ability to effectively navigate complex and dynamic environments is significantly enhanced, ultimately leading to more robust and reliable performance.

    In SAC, we define the action-value function as follows:

    \begin{align}
    	\begin{split}
    		q_{\pi}(s_{KF}, a) &= \mathbb{E}_{r,s^{'}_{KF}\sim P(s_{KF},a),a^{'}\sim \pi(s^{'}_{KF})} \left[ r + \gamma \left( q_{\pi}(s^{'}_{KF}, a^{'}) \right. \right. \\
    		&\quad \left. \left. + \alpha H\left(\pi(.|s^{'}_{KF})\right) \right) \right]
    	\end{split}
    \end{align}
    

    The expectation is over the reward, next state, and next action. We’re going to add up the reward and the discounted value of the next state-action pair. However, we add the entropy of the policy at the next state. Alpha tunes the importance we give to the entropy term.

    \begin{align}
        \textbf{\textit{SAC}}^{target} = r + \gamma \left[\min_{n}Q(s^{'}_{KF}, \hat{a}^{'};\theta^{n,-})-\alpha log \pi(\hat{a}^{'}|s^{'}_{KF};\phi) \right]
    \end{align}


    \begin{align}
        J_{\pi}(\phi) = \mathbb{E}_{s_{KF}\sim u(D), \hat{a}\sim \pi} \left[\min _ {n} Q\left(s_{KF}, \hat{a};\theta^{n}\right) - \alpha log \pi(\hat{a}|s_{KF};\phi)\right]
    \end{align}

    \begin{align}
        J(\alpha) = \mathbb{E}_{s_{KF}\sim u(D), \hat{a}\sim \pi}\left[\alpha \left(\textit{H} + log \pi (\hat{a}|s_{KF};\phi)\right)\right]
    \end{align}

    
    \subsubsection{Proximal Policy Optimization (PPO)}
        The Proximal Policy Optimization (PPO) \cite{DBLP:journals/corr/SchulmanWDRK17} algorithm represents a notable advancement in the field of reinforcement learning, specifically targeting on-policy methods. PPO's major contribution is the introduction of a surrogate objective function, which is a fundamental departure from traditional on-policy approaches such as the Advantage Actor-Critic (A2C). The surrogate objective function enables the algorithm to take multiple gradient steps using the same mini-batch of experiences, whereas other on-policy methods like A2C necessitate the immediate disposal of experience samples after a single optimization step \cite{PPO}.

        The core concept behind PPO's surrogate objective function is that it allows for multiple policy updates using the same set of experiences. This is achieved by comparing the new policy to the old policy using a likelihood ratio, where the ratio represents the probability of taking an action under the new policy divided by the probability of taking that action under the old policy. The objective function aims to maximize the expected return while keeping the policy updates within a certain trust region, meaning the new policy should not deviate significantly from the old policy.

        PPO's clipped objective function plays a crucial role in controlling the magnitude of policy updates. The clipping mechanism prevents the policy from undergoing drastic changes after each optimization step, thereby ensuring that updates are conservative and controlled. This approach has two major benefits.

        First, it addresses the problem of performance collapse, a prevalent issue in on-policy policy gradient methods. Performance collapse occurs when the policy updates are too aggressive, causing the algorithm to lose the progress made in previous iterations. By constraining policy updates, PPO's clipped objective function reduces the risk of performance collapse, resulting in a more stable learning process.

        Second, the conservative optimization approach allows for the reuse of mini-batches of experiences in multiple optimization steps. This is a significant advantage over other on-policy methods, as it leads to improved sample efficiency. The ability to reuse experiences means that the algorithm can extract more information from each sample, reducing the overall number of samples required for effective learning.

        In conclusion, the Proximal Policy Optimization algorithm presents a significant improvement over traditional on-policy methods in reinforcement learning. Its innovative surrogate objective function allows for multiple gradient steps using the same mini-batch of experiences, and the clipped objective function ensures conservative policy updates. These features contribute to mitigating performance collapse and enhancing sample efficiency, making PPO a powerful and reliable choice for many reinforcement learning problems.

        Proximal Policy Optimization (PPO) can be thought of as an enhancement to the Advantage Actor-Critic (A2C) algorithm. It is crucial to understand that PPO should not be confused with being an improvement to Soft Actor-Critic (SAC). While PPO shares similarities with other reinforcement learning algorithms such as Deep Deterministic Policy Gradient (DDPG), Twin Delayed Deep Deterministic (TD3), and SAC, it has a different lineage and purpose.

        PPO and A2C both utilize the actor-critic architecture, where an actor is responsible for making decisions based on the current policy, and a critic estimates the value function to help update the policy. The key distinction between PPO and A2C lies in the way PPO handles policy updates, allowing for multiple gradient steps using the same mini-batch of experiences, resulting in improved sample efficiency and stability.

        On the other hand, TD3 and SAC are both offshoots of DDPG, an algorithm that combines ideas from deep learning and reinforcement learning to handle continuous control problems. TD3 is a direct improvement over DDPG, addressing issues such as overestimation bias and instability. SAC, although developed concurrently with TD3, shares several features with it. The second version of the SAC paper even incorporated some of the ideas from TD3. However, it is important to note that SAC is not a direct improvement to TD3; rather, it is a distinct algorithm with a focus on maximizing the entropy of the policy, enabling better exploration.


        In summary, Proximal Policy Optimization is an enhancement of the Advantage Actor-Critic algorithm, sharing the same actor-critic architecture and building upon the A2C framework. While PPO shares certain similarities with other reinforcement learning algorithms such as DDPG, TD3, and SAC, it is crucial to recognize that PPO is specifically an improvement to A2C, and not to the other algorithms.

        \begin{align}
        \begin{split}
        	J\left(\phi, \phi ^{-}\right) &= \mathbb{E}_{\left(s_{KF}, a, A^{GAE}\right) \sim u (D(\phi ^{-}))} \left\{ \min \left[ \frac{\pi(a|s_{KF};\phi)}{\pi(a|s_{KF};\phi^{-})}A^{GAE}, \right. \right. \\
        	&\quad \left. \left. clamp\left(\frac{\pi(a|s_{KF};\phi)}{\pi(a|s_{KF};\phi^{-})}, 1-\epsilon, 1+\epsilon\right)A^{GAE} \right] \right\}
        \end{split}
        \end{align}
        

        \begin{align}
        	\begin{split}
        		L\left(\phi, \phi^{-}\right) &= \mathbb{E}_{\left(s_{KF},a,G,V \right) \sim u \left(D(\theta^{-})\right)} \left\{ \max \left[ G-V(s_{KF};\theta), \right. \right. \\
        		&\quad \left. \left. G - \left (V + clamp \left(V(s_{KF};\theta) - V, -\delta, \delta \right) \right) \right] \right\}
        	\end{split}
        \end{align}
        
\section{Introduction to Kalman Filtering Algorithms}
A Kalman Filter is able to update and estimate of an evolving state. The goal of the Kalman Filter is to take a probabilistic estimate of an state and update it in real time using two steps, prediction and correction. In the following some extended of Kalman Filter algorithm are introduced in details.

\subsection{Standard Kalman Filter (SKF)}

\subsubsection{Introduction}
The Kalman Filter requires the following motion and measurement models:
\begin{itemize}
	\item Motion Model:
	\begin{align}
		x_k = F_{k-1}x_{k-1} + G_{k-1}u_{k-1} + w_{k-1}
	\end{align}
	in which $u_{k-1}$ is an external signal that affects the evolution of our system state (e.g. the acceleration in three dimensions to change the speed of a drone).
	
	\item Linear Measurement Model:
	\begin{align}
		y_{k} = H_{k}x_{k} + v_{k}
	\end{align}
	
\end{itemize}

and the following noise properties are considered for this model:

\begin{itemize}
	\item Measurement noise:
	$$v_{k} \sim \textit{N}(0,R_{k})$$
	\item Process or motion noise:
	$$w_{k} \sim \textit{N}(0,Q{k}))$$
	which governs how certain we are that our linear dynamic system is actually correct and how uncertain we are about the effects of our control inputs.
\end{itemize}

The Kalman Filter is a recursive least square estimator that also includes a  motion model.
The following steps should be taken to find the estimates of the state we are focusing on:

\begin{itemize}
	\item Prediction:
	\begin{align}
		\check{x}_{k} = F_{k-1}x_{k-1} + G_{k-1}u_{k-1}
	\end{align}
	
	\begin{align}
		\check{P}_{k} = F_{k-1} \hat{P}_{k-1}F^{T}_{k-1} + Q_{k-1}
	\end{align}
	\item Update the Optimal Gain:
	\begin{align}
		K_{k} = \check{P}_{k}H^{T}_{k}(H_{k} \check{P}_{k}H^{T}_{k} + R_{k})^{-1}
	\end{align}
	\item Correction:
	\begin{align}
		\hat{x}_{k}  =\check{x}_{k} + K_{k}(y_{k}-H_{k} \check{x}_{k})
	\end{align}
	
	\begin{align}
		\check{P}_{k} = (1-K_{k}H_{k})\check{P}_{k}
	\end{align}
	
\end{itemize}

in which,

\begin{itemize}
	\item $y_{k}-H_{k} \check{x}_{k}$ is the innovation matrix
	\item $\check{P}_{k}$ is the corrected state covariance matrix
	\item $\check{x}_{k}$ is the prediction given motion model at time $k$
	\item $\hat{x}_{k}$ is the corrected prediction give measurement at time $k$
\end{itemize}

A smaller state covariance means we are more certain about the drone's position after we incorporate the position measurement and the measurement noise variance is quite small.

\subsubsection{Bias in State Estimation}
We say an estimator or a filter is unbiased if it produces an \textit{average} error of zero at a particular time step $k$ over many trials.
So, we drive our system like a drone for $k$ time steps and record the following data to get the estimation errors and repeat this process.

\begin{itemize}
	\item Process and Motion:
	$$x_k = F_{k-1}x_{k-1} + G_{k-1}u_{k-1} + w_{k-1}$$
	\item Measurement:
	$$y_{k} = H_{k}x_{k} + v_{k}$$
\end{itemize}

\begin{figure}[h]
	\centering
	\includegraphics[width=.8\linewidth]{images/kalman_filter/PDF.png}
	\caption{Definition of Bias on Probability Distribution Function}
	\label{fig:PDF_def}
\end{figure}

where in \ref{fig:PDF_def}, the $P_{k}$ is the true position of the drone in 3D environment and $E[\hat{P}_{k}]$ is the mean of the estimated position values.
To compute the bias for the Kalman Filter we have the error dynamics as:

\begin{itemize}
	\item Predicted State Error:
	$$\check{e}_{k} = \check{x}_{k} - x_{k}$$
	\item Corrected Estimate Error:
	$$\hat{e} = \hat{x}_{k} - x_{k}$$
\end{itemize}

By using the Kalman Filter equations, we can derive:

\begin{itemize}
	\item Predicted State Error:
	$$\check{e}_{k} = F_{k-1}\check{e}_{k-1} - w_{k}$$
	\item Corrected Estimate Error:
	$$\hat{e} = (1-K_{k}H_{k})\check{e}_{k} + K_{k}v_{k}$$
\end{itemize}

The expectation values of these errors is equal to zero.For this to be true, we need to ensure that our initial state estimate is unbiased and our noise is white, uncorrelated, and zero mean. So, for the Kalman Filter for all $k$ steps:

\begin{itemize}
	\item Expectation of the Predicted State Error:
	$$E[\check{e}_{k}] = E[F_{k-1}\check{e}_{k-1} - w_{k}] = F_{k-1}E[\check{e}_{k-1}] - E[w_{k}] = 0$$
	\item Expectation of the Corrected Estimate Error:
	$$E[\hat{e}] = E[(1-K_{k}H_{k})\check{e}_{k} + K_{k}v_{k}]=(1-K_{k}H_{k})E[\check{e}_{k}] + K_{k}E[v_{k}]=0$$
\end{itemize}

This does not mean that the error on a give trial will be zero, but that with enough trials, our expected error is zero.

\subsubsection{Consistency in State Estimation}
By consistency we mean that for all time steps $k$, the filter covariance $\hat{P}_{k}$ matches the expected value of the square of our error. Therefore, a filter is consistent if for all $k$ we have:

\begin{align}
	E[\hat{e}_{k}^{2}] = E[(\hat{P}_{k}-P_{k})^{2}]= \hat{P}_{k}
\end{align}

The empirical variance of our estimate should match the variance reported by the filter. This means that our filter is neither overconfident, nor underconfident in the estimate it has produced.
A filter that is over confident, and hence inconsistent, will report a covariance that is optimistic. Therefore, the filter will essentially place too much emphasis on its own estimate will be less sensitive to future measurement updates which may provide critical information.
So long as our initial estimate is consistent and we have white zero mean noise, then all estimates will be consistent.

\begin{equation}
	\left.
	\begin{aligned}
		E[\hat{e}_{0}\hat{e}_{0}^{T}] = \check{P}_{0} , E[v] = 0\\
		E[w] = 0, \text{White noise} 
	\end{aligned}
	\right\} \Rightarrow E[\check{e}_{k}\check{e}_{k}^{T}] = \check{P}_{k}, E[\hat{e}_{k}\hat{e}_{k}^{T}] = \hat{P}_{k}
\end{equation}

We have shown that given our linear formulation, and zero mean white noise, the Kalman Filter is unbiased. 
We can also say that the filter is consistent if:
$$E[\hat{e}_{k}] = 0$$
$$E[\hat{e}_{k}\hat{e}_{k}^{T}] = \hat{P}_{k}$$

In general, if we have white, uncorrelated zero mean noise, the Kalman Filter is the best(Lowest Variance) unbiased estimator that uses only a linear combination of measurements. Therefore, the Kalman Filter is the best linear unbiased estimator. 

\subsection{Extended Kalman Filter (EKF)}
\subsubsection{Introduction}
The Extended Kalman Filter uses the first-order linearization to turn a nonlinear problem into a linear one. This algorithm talks about the role of the Jacobian matrices and how to compute them.
Actually, Linear systems do not exist in reality. So, there is a question about how we can adapt the Kalman Filter algorithm to nonlinear discrete-time systems?

We have:

\begin{align}
	x_{k} = f_{k-1}(x_{k-1}, u_{k-1}, w_{k-1})
\end{align}

\begin{align}
	y_{k} = h_{k}(x_{k}, v_{k})
\end{align}

in which the $x_{k}$ is the dynamic of the system and $y_{k}$ is the measurement model. 

\begin{figure}[h]
	\centering
	\includegraphics[width=.8\linewidth]{images/kalman_filter/EKF.png}
	\caption{Nonlinear system and the meaning of first order linearization}
	\label{fig:EKF}
\end{figure}
To linearize a nonlinear system, firstly a point should be chosen like point $a$, and the approximation of the nonlinear function by a tangent line at that point should be derived. So, the EKF can be defined as the Linearized Kalman Filter. 

Suppose a nonlinear $f(x)$ function is given. Mathematically, the linear approximation is computed using a first-order Ttaylor expansion as:

\begin{equation}
	\begin{aligned}
		f(x) \approx f(a) + \frac{\partial f(x)}{\partial x} \Big|_{x=a} (x - a) + \frac{1}{2!} \frac{\partial^2 f(x)}{\partial x^2} \Big|_{x=a} (x - a)^2 + \frac{1}{3!} \frac{\partial^3 f(x)}{\partial x^3} \Big|_{x=a} (x - a)^3 + \dots
	\end{aligned}
\end{equation}
and the first-order Taylor expansion would be as:

$$f(x) \approx  f(a) + \frac{\partial f(x)}{\partial x} \Big|_{x=a} (x - a)$$

\subsubsection{Algorithm}
The procedure to derive the equations of EKF is going to be explained in details in the following.
\begin{itemize}
	\item Linearize Motion Model:\\
	For the EKF, the operating point is chosen as the most recent state estimated point with known input and zero noise. Therefore, we have linearized motion model for EKF as follows:
	
	\begin{equation}
		\begin{aligned}
			x_{k} &= f_{k-1}(x_{k-1}, u_{k-1}, w_{k-1}) \approx f_{k-1}(\hat{x}_{k-1}, u_{k-1}, 0) \\
			&+ \frac{\partial f_{k-1}}{\partial x_{k-1}} \Big|_{(\hat{x}_{k-1}, u_{k-1}, 0)} (x_{k-1} - \hat{x}_{k-1}) \\
			&+ \frac{\partial f_{k-1}}{\partial w_{k-1}} \Big|_{(\hat{x}_{k-1}, u_{k-1}, 0)} w_{k-1}
		\end{aligned}
	\end{equation}
	the two last terms are named as:
	$$F_{k-1} = \frac{\partial f_{k-1}}{\partial x_{k-1}} \Big|_{(\hat{x}_{k-1}, u_{k-1}, 0)}$$
	$$L_{k-1} = \frac{\partial f_{k-1}}{\partial w_{k-1}} \Big|_{(\hat{x}_{k-1}, u_{k-1}, 0)}$$
	\item Linearize Measurement Model:\\
	the linearized measurement model will be as the following:
	
	\begin{equation}
		\begin{aligned}
			y_{k} &= h_{k}(x_{k},v_{k}) \approx h_{k}(\check{x}_{k},0) \\
			&+ \frac{\partial h_{k}}{\partial x_{k}} \Big|_{(\check{x}_{k}, 0)} (x_{k} - \check{x}_{k}) + \frac{\partial h_{k}}{\partial v_{k}} \Big|_{(\check{x}_{k}, 0)} v_{k}
		\end{aligned}
	\end{equation}
	
	and the two last terms are named as:
	$$H_{k} = \frac{\partial h_{k}}{\partial x_{k}} \Big|_{(\check{x}_{k}, 0)}$$
	$$M_{k} = \frac{\partial h_{k}}{\partial v_{k}} \Big|_{(\check{x}_{k}, 0)}$$
	
	\item Computing the Jacobian Matrices:\\
	Now, we have linear system in state-space and the $F_{k-1}, L_{k-1}, H_{k}, M_{k}$ are called the Jacobian matrices of the system. A Jacobian matrix is the matrix of all first-order partial derivatives of a vector-valued function.
	
	\begin{equation}
		\frac{\partial f}{\partial x} = [\frac{\partial f}{\partial x_{1}} \cdots \frac{\partial f}{\partial x_{n}}] =
		\begin{bmatrix}
			\frac{\partial f_1}{\partial x_1} & \frac{\partial f_1}{\partial x_2} & \cdots & \frac{\partial f_1}{\partial x_n} \\
			\frac{\partial f_2}{\partial x_1} & \frac{\partial f_2}{\partial x_2} & \cdots & \frac{\partial f_2}{\partial x_n} \\
			\vdots & \vdots & \ddots & \vdots \\
			\frac{\partial f_m}{\partial x_1} & \frac{\partial f_m}{\partial x_2} & \cdots & \frac{\partial f_m}{\partial x_n}
		\end{bmatrix}
	\end{equation}
	
	Consider each column of the Jacobian matrix contains the derivatives of the function outputs with respect to a given input.
	Intuitively, the Jacobian matrix tells us how fast each output of the function is changing along each input dimensions.
	
	\item Write the final equations:\\
	With the linearized models and Jacobians, we can now use the Kalman Filter equations which derived as:
	$$x_{k} = f_{k-1}(x_{k-1}, u_{k-1}, 0) + F_{k-1}(x_{k-1}-\hat{x}_{k-1})+L_{k-1}w_{k-1}$$
	$$y_{k} = h_{k}(\check{x}_{k},0) + H_{k}(x_{k}-\check{x}_{k}) + M_{k}v_{k}$$
	\begin{itemize}
		\item Prediction:
		\begin{align}
			\check{x}_{k} = f_{k-1}(\hat{x}_{k-1}, u_{k-1}, 0)
		\end{align}
		\begin{align}
			\check{P}_{k} = F_{k-1}\hat{P}_{k-1}F_{k-1}^{T} + L_{k-1}Q_{k-1}L_{k-1}^{T}
		\end{align}
		\item Optimal Gain:
		\begin{align}
			K_{k} = \check{P}_{k}H_{k}^{T}(H_{k}\check{P}_{k}H_{k}^{T} + M_{k}R_{k}M_{k}^{T})^{-1}
		\end{align}
		\item Correction:
		\begin{align}
			\hat{x}_{k} = \check{x}_{k} - K_{k}(y_{k} - h_{k}(\check{x}_{k},0))
		\end{align}
		\begin{align}
			\hat{P}_{k} = (1-K_{k}H_{k})\check{P}_{k}
		\end{align}
		in which:\\
		$\check{x}_{k}$ is the prediction given motion model at time $k$\\
		$\hat{x}_{k}$ is the corrected prediction give measurement at time $k$
		
	\end{itemize}
	Consider the motion model is linearized about the previous state estimate and the measurement model is linearized about the predicted state.
\end{itemize}

\subsection{Single-Instruction Multi-data Kalman Filter}
The Kalman filter is a mathematical algorithm that provides an efficient computational solution to estimate the state of a process in a way that minimizes the mean of the squared error. It is used in a wide range of engineering \cite{KF} and data analysis applications to filter out noise and provide accurate data about the state of a system over time.

The Kalman filter becomes even more powerful when integrated with the SIMD concept. The SIMD (Single Instruction, Multiple Data) Kalman filter leverages the power of modern processors to perform multiple operations simultaneously. By packing several data items into one register and operating on all of them simultaneously, the SIMD Kalman filter significantly increases the data processing speed. This approach is particularly beneficial in scenarios where operations can be naturally parallelized, such as in high-energy physics experiments where large volumes of data need to be processed quickly. Using the SIMD Kalman filter, these operations can be performed much faster, leading to more efficient data processing and analysis.

The SIMD (Single Instruction, Multiple Data) Kalman filter works by leveraging the power of modern processors to perform multiple operations simultaneously. The SIMD Kalman filter can process multiple data streams against a single instruction stream, particularly beneficial in scenarios where operations can be naturally parallelized. This makes it especially useful in high-energy physics experiments, where large volumes of data need to be processed quickly.
The SIMD (Single Instruction, Multiple Data) Kalman filter operates in several steps to leverage the power of modern processors for efficient data processing. Initialization: The algorithm begins with an initial approximation of the system's state. This approximation is represented as a vector, and its covariance matrix is set to a large positive number, indicating a high level of uncertainty. Prediction: The algorithm predicts the state of the system at the next time step. This prediction is based on the current state and the system's dynamics, represented by the prediction matrix. The prediction step produces an estimated state vector and its associated covariance matrix, which represents the uncertainty of the prediction. Process Noise: This step accounts for the probabilistic deviations in the system's state due to process noise. The estimated state vector and its covariance matrix are updated to reflect this noise. Filtration: The state vector is updated with the new measurement to get the optimal estimate of the system's state and its covariance matrix. The Kalman gain matrix, which determines how much weight to give to the new measurement based on its uncertainty, is calculated and applied. The total deviation of the obtained estimation from the measurements is also calculated. The filter's complete mathematical formulations are reported in \cite{SIMD}. 

\begin{figure}[!h]
	\centering
	\includegraphics[height=35mm]{images/kalman_filter/kalman_filter.png}
	\caption{Kalman Filtering Block Diagram}
\end{figure}

In the context of this paper, the SIMD form of the Kalman filter has been used to filter the information about the position of the gates. This did allow us to obtain, after a small number of observations, an accurate estimate of the gate's movement. The downstream algorithm later used this information to estimate the position of the gate at a given moment in time.

In the study discussed in this paper, the SIMD version of the Kalman filter has been employed to filter the data related to the position of the gates. This application enabled us to acquire a precise estimate of the gate's motion after a limited number of observations. The downstream algorithm subsequently utilized this information to approximate the gate's position at a specific time point. In the prediction step of the Kalman filter, an estimate of the new position of the gate is made based on the previous state. This step incorporates the state transition model and the control input, if any, to predict the current state of the gate. This prediction forms the prior estimate for the state of the system. The observations from the environment then come into play during the update step. These observations are used to adjust the prediction based on what was measured, which can be particularly beneficial when there is uncertainty or noise in the system. In this case, the measurements pertain to the gate's movement. The difference between the prediction from the prior state and the observation from the environment, also known as the innovation or residual, is then used to update the state estimate. This updated estimate is a weighted average of the prior estimate and the current measurement, with more weight given to estimates with more certainty. Through several iterations, the above process yields an accurate estimate of the gate's movement, effectively tracking the gate's position over time. This ability to use observations to predict and correct the position estimate makes the Kalman filter an excellent tool for dealing with the dynamics of moving objects like the gate in our study.

