Teng Ma

Teng Ma

An Introduction to Error state Kalman Filter

8 minute read

Published:

Error state is the difference between True state $x_t$ and Nominal state $x$. The nominal state does not take into account the noise, just derived from motion equations. As a result there will be accumulate errors, which are collected in the error-state $\delta x$ and the ESKF will estimate these errors. In parallel with integration of the nominal state, the ESKF predicts a Gaussian estimate of the error-state, and inject the mean of error state back to nominal-state.
When the IMU measurement data arrives, we integrate it and put it into the nominal state variable. Since this approach does not consider noise, the result will naturally drift quickly, so we hope to put the error part as an error variable in ESKF. ESKF will consider the effects of various noises and zero offsets, and give a Gaussian distribution description of the error state. At the same time, ESKF itself, as a Kalman filter, also has a prediction process and a correction process, and the correction process needs to rely on sensor observations other than the IMU. After the correction, ESKF can give the posterior error Gaussian distribution, and then we can put this part of the error into the nominal state variable, and set ESKF to zero, thus completing a loop.

Why ESKF?

CLICK ME In most modern IMU systems, people often use Error state Kalman filter (ESKF) instead of the original state Kalman filter. Since the error-state are small signal, ESKF has several advantages:
1. Regarding of the rotation part, the state variables of ESKF can be expressed by three-dimensional variables, the minimized parameters that are used to express the increment of rotation. While the traditional KF needs to use quaternion (4-dimensional) or higher-dimensional expression (rotation matrix, 9-dimensional), or it has to use a singular expression (Euler angle).
2. ESKF is always near the origin, far away from the singular point, and it will be perfect to perform linearization approximation because it near the operating point.
3. The error-state is always small, meaning that all second-order products are negligible. This makes the computation of Jacobians very easy and fast.
4. The kinematics of the error state is also smaller than the original state variable, because we can put a large number of updated parts into the original state variable.


Equation of State

CLICK ME Good to see all the variables used in a table, all variable names are consistent with the refered Joan's paper: ### Variable List |Magnitude |True |Nominal |Error |Composition|Measured |Noise | | -------- | -------- | -------- | -------- | --------- |-------- |-------- | |Full state| $x_t$ | $x$ | $\delta x$ | $x_t = x \oplus \delta x$ | |Position | $p_t$ | $p$ | $\delta p$ | $p_t = p + \delta p$ | |Velocity | $v_t$ | $v$ | $\delta v$ | $v_t = v + \delta v$ | |Rotation matrix| $R_t$ | $R$ | $\delta R$ | $R_t = R \delta R$ | |Angles vector||| $\delta \theta$ | $\delta R = exp[\delta \theta]$\^ | |Accelerometer bias| $a_{bt}$ | $a_b$ | $\delta a_b$ | $a_{bt} = a_b + \delta a_b$ || $a_\omega$ | |Gyrometer bias|$\omega_{bt}$|$\omega_b$|$\delta \omega_b$|$\omega_{bt} = \omega_b + \delta \omega_b$|| $\omega_\omega$ | |Gravity vector|$g_t$|$g$|$\delta g$|$g_t = g + \delta g$| |Acceleration| $a_t$ |||| $a_m$ | $a_n$ | |Angular rate| $\omega_t$ |||| $\omega_m$ | $\omega_n$ | True state $x_t$ in ESKF: $x_t = [p_t, v_t, R_t, a_{bt}, \omega_{bt}, g_t]^T$, $x_t$ change over time and the subscript $t$ denotes true state. We record the IMU readings as $a_m, \omega_m$, which are perturbed by the white Gaussian noise $a_n, \omega_n$, and $a_\omega$ and $\omega_\omega$ are noise of the bias of IMU. Now we can write the relationship between the derivative of the state variable with respect to the observed measurement (angular velocity is defined in the local reference, the common case in IMU): $$ \dot{p_t} = v_t\\ \dot{v_t} = R_t(a_m-a_{bt}-a_n)+g_t\\ \dot{R_t} = R_t(\omega_m -\omega_{bt}-\omega_n) \hat{} \\ \dot{a_{bt}} = a_\omega\\ \dot{\omega_{bt}} = \omega_\omega \\ \dot{g_t} = 0 $$ Nominal state $x$ kinematics corresponds to the modeled system without noise or perturbations, $$ \dot{p} = v\\ \dot{v} = R(a_m-a_b)+g\\ \dot{R} = R(\omega_m-\omega_b) \hat{}\\ \dot{a_b} = 0\\ \dot{\omega_b} = 0\\ \dot{g_t} = 0 $$ Then we have error state $\delta x$ kinematics: $$ \dot{\delta p} = \delta v \\ \dot{\delta v} = - R(a_m - a_b) \hat{} \delta \theta - R \delta a_b + \delta g - R a_n \\ \dot{\delta \theta} = -(\omega_m - \omega_b) \hat{} \delta \theta - \delta \omega_b - \omega_n \\ \dot{\delta a_b} = a_\omega\\ \dot{\delta \omega_b} = \omega_\omega\\ \dot{\delta g} = 0 $$ The discrete form of error state kinematics: $$ \delta p = \delta p + \delta v \Delta t \\ \delta v = \delta v + (- R(a_m - a_b) \hat{} \delta \theta - R \delta a_b + \delta g) \Delta t + v_i \\ \delta \theta = exp(-(\omega_m-\omega_b)\delta t) \delta \theta - \delta \omega_b \Delta t + \theta_i\\ \delta a_b = \delta a_b + a_i\\ \delta \omega_b = \delta \omega_b + \omega_i\\ \delta g = \delta g $$ where $exp(-(\omega_m-\omega_b)\delta t)$ means the Lie algebra of the incremental rotation, $v_i, \theta_i, a_i, \omega_i$ are the random impulses applied to the velocity, orientation and acceleration and angular rate estimates, modeled by white Gaussian processes. Their mean is zero, and their covariances matrices are obtained by integrating the covariances of $a_n, \omega_n, a_\omega, \omega_\omega$ over the step time $\Delta t$.


Prediction and updating equations

CLICK ME Now we have the motion equation in discrete time domain, $$ \delta x = f(\delta x) + \omega, \omega \sim N(0, Q) $$ $\omega$ is noise, which is composed by $v_i, \theta_i, a_i, \omega_i$ mentioned above, so $Q$ matrix should be: $$ Q = diag(0^3,cov(v_i), cov(\theta_i), cov(a_i), cov(\omega_i),0^3) $$ The prediction equations are written: $$ \delta x_{pred} = F \delta x\\ P_{pred} = FPF^T+Q $$ where $F$ is the Jaccobian of the error state function $f$, the expression is detailed below: $$ \begin{bmatrix} I & I \Delta t &0&0&0&0\\ 0 & I & - R(a_m - a_b) \hat{} \Delta t &-R \Delta t&0&I \Delta t\\ 0&0& exp(-(\omega_m-\omega_b)\Delta t)&0&-I \Delta t&0\\ 0&0&0&I&0&0\\ 0&0&0&0&I&0\\ 0&0&0&0&0&I\\ \end{bmatrix} $$ Suppose an abstract sensor can produce observations of state variables, and its observation equation is written as: $$ z = h(x) + v, v \sim N(0, V) $$ where $h$ is a general nonlinear function of the system state (the true state), and $v$ is measurement noise, a white Gaussian noise with covariance $V$. The updating equations are: $$ K = P_{pred} H^T (H P_{pred} H^T + V)^{-1}\\ \delta x = K (z - h(x_t))\\ P = (I - K H) P_{pred} $$ Where $K$ is Kalman gain, $P_{pred}$ is prediction covariance matrix, $P$ is covariance matrix after updating and $H$ is defined as the Jacobian matrix of measurement equation of error state, according to chain rule $$ H = \frac{\partial h}{\partial \delta x}=\frac{\partial h}{\partial x} \frac{\partial x}{\partial \delta x} $$ First part $\frac{\partial h}{\partial x}$ can be easily obtained by linearizing the measurement equation, the second part $\frac{\partial x}{\partial \delta x}$ is the Jacobian of the true state with respect to the error state, which is the combination of 3x3 identity matrix (for example, $\frac{\partial (p+ \delta p)}{\partial \delta p} = I_3$), expect for the rotation part, in quaternion form it is $\frac{\partial (q \otimes \delta q)}{\partial \delta \theta}$, here in the form of rotation matrix in $SO3$, it is $\frac{\partial log (R Exp(\delta \theta))}{\partial \delta \theta}$, where $Exp(\delta \theta)$ is the Lie algebra of rotation $\delta R$, $H$ can be obtained according to Baker–Campbell–Hausdorff (BCH) formula. Updating the state and reset the error state: $$ x_{k+1} = x_k \oplus \delta x_k\\ \delta x_k = 0 $$ where $\oplus$ is defined addition operation to simplify the following equations: $$ p_{k+1}=p_k+ \delta p_k\\ v_{k+1}=v_k+ \delta v_k\\ R_{k+1}=R_kExp(\delta \theta_k)\\ a_{b,k+1}=a_{b,k}+\delta a_{b,k}\\ \omega_{b,k+1}=\omega_{b,k}+\delta \omega_{b,k}\\ g_{k+1}=g_k+\delta g_k\\ $$


For more derivations and details about quanterion representation in rotation part, please read Joan’s paper: Quaternion kinematics for the error-state Kalman filter, which is also the main reference of this blog.

You May Also Enjoy

Hungarian Algorithm in multiple object tracking

7 minute read

Published:

Facilitated by the development of deep learning in subfields of computer vision including single object tracking (SOT), video object detection (VOD), and re-identification (Re-ID), many methods of multi-object tracking (MOT) have emerged, such as various embedding models designed for object locations and track identities [1]. While tracking-by-detection is still an effective paradigm for MOT, since we can always combine off-the-shelf state-of-the-art detectors and Re-ID models to improve the MOT system. The idea is: detect the objects in the current frame, then try to associate the detected boxes with tracked boxes in the previous frames. In practice, we need a more sophisticated design (e.g. choose similarity indices for data association, use Kalman Filter to predict new locations of tracks etc.) and proper strategies (e.g. to delete or initialize tracks). Hunarian algorithm is commonly used to matching objects after we get the similarity matrix, in this blog, I will try to explain how and why Hunarian algorithm works from the perspective of linear programming.

Problem Description

Hunarian algorithm or Kuhn-Munkres algorithm is well known for solving assignment problem, which involves assigning tasks to agents (or jobs to workers) such that the total cost is minimized (or the total profit is maximized), subject to the constraint that each agent is assigned exactly one task, and each task is assigned to exactly one agent.

Primal Form of the Assignment Problem

We are given an $n \times n$ cost matrix $C$, where $c_{ij}$ is the cost of assigning agent $i$ to task $j$.
\(\begin{aligned} \text{min} & \sum_{i \in I} \sum_{j \in J} c_{ij}x_{ij} \\ s.t. & \sum_{j \in J}x_{ij} = 1 \text{ for all } i \in I \\ &\sum_{i \in I}x_{ij} = 1 \text{ for all } j \in J \\ & x_{ij} \geq 0 \end{aligned}\)

Note that $x_{ij} = 1$ if i is assigned to j and 0 otherwise.

Assignment Problem is a classic linear programming problem, since both the objective function and constraints are linear sums, even though it involves binary decision variables. We can model the problem as a bipartite graph and interpret the algorithm from the perspective of graph (find the perfect matching by finding augmenting path), but this blog will not cover this topic. Let’s try to understand Hungarian algorithm under the framework of linear programming. Since the primal problem is tricky to deal with, let’s go for the dual form of the problem.

Primal to Dual To convert the primal Assignment Problem to its dual form, we need to use Lagrange multipliers to relax the equality constraints in the primal problem. Introduce $u_i$ as the Lagrange multiplier associated with the constraint that agent $i$ is assigned exactly one task ($\sum_j x_{ij} = 1$), $v_j$ as the Lagrange multiplier associated with the constraint that task $j$ is assigned to exactly one agent ($\sum_i x_{ij} = 1$), then formulate the Lagrangian:
$$ L(x_{ij},u_i,v_j)=\sum_{i=1}^n \sum_{j=1}^n c_{ij}x_{ij}+\sum_{i=1}^n u_i (1-\sum_{j=1}^n x_{ij})+\sum_{j=1}^n v_j (1-\sum_{i=1}^n x_{ij}) $$ Lagrange dual function is defined as the minimum value of the Lagrangian over primal variables:
$$ g(u,v)=\text{inf}_x L(x,u,v) $$ where $\text{inf}_x$ means the infimum (the lower bound) of the Lagrangian over $x$. Since the dual function is the pointwise infimum of a family of affine functions of (u, ν), it is concave, even when the problem is not convex [2]. To minimize this Lagrangian $L(x_{ij},u_i,v_j)$ with respect to $x_{ij}$, rewrite the Lagrangian as:
$$ L(x_{ij},u_i,v_j)=\sum_{i=1}^n \sum_{j=1}^n [(c_{ij}-u_i-v_j)x_{ij}] + \sum_{i=1}^n u_i + \sum_{j=1}^n v_j $$ The minimization occurs in the condition:
$$ x_{ij}=1 \ \text{ if }\ c_{ij} \leq u_i+v_j \\ x_{ij}=0 \ \text{ if }\ c_{ij} > u_i+v_j $$ Now we want to find the tightest lower bound or minimum duality gap, so we need to maximize the dual function w.r.t $u$ and $v$. In the case of $x_{ij}=1$ If $c_{ij} \leq u_i+v_j$, the maximum occurs when $c_{ij} = u_i+v_j$, so $c_{ij}-u_i-v_j$ in Lagrangian is non-negative.


Dual Form of the Assignment Problem

The dual form of the Assignment Problem can be written as:
\(\begin{aligned} \text{max} &\sum_{i=1}^n u_i + \sum_{j=1}^n v_j \\ s.t. & u_i+v_j \leq c_{ij}, \ \forall i, j \end{aligned}\)

where $u_i$ is the dual variable associated with agent $i$’s assignment constraint, $v_j$ is the dual variable associated with task $j$’s assignment constraint.
The strong duality theorem of linear programming guarantees that if a feasible and bounded solution exists for the primal linear problem, the dual linear problem is feasible (and bounded as well, by weak duality), with the same optimum value as the primal [3]. The primal problem has constraints that ensure every agent is assigned exactly one task, and every task is assigned exactly one agent, a feasible primal solution exists (since the problem is well-posed with the same number of agents and tasks).

Hungarian Algorithm

Given a $n \times n$ cost matrix (Adjacent Matrix in Graph theory) $C$, the algorithm is summarized as 5 steps below:

  1. Find and subtract the row minimum, namely find the minimum element in each row of the cost matrix and subtract the minimum element in each row from the element in that row.
  2. Find and subtract the colume minimum, namely find the minimum element of each column in the remaining matrix and subtract the minimum element in each column from the elements in that column.
  3. Use the minimum number of horizontal or vertical lines to cover all zero elements in the matrix. If the minimum number of lines is equal to $n$, stop running; if the minimum number of lines is less than $n$, continue to the next step.
  4. Find the minimum value among the uncovered elements, subtract this minimum value from the uncovered elements, and add this minimum value to the elements at the intersection of different lines.
  5. Go back to step 3, that is, continue to cover all zero elements in the matrix with the minimum number of horizontal or vertical lines. If the minimum number of lines is equal to $n$, stop running; if the minimum number of lines is less than $n$, execute step 4.

For details of $O(n^3)$ algorithm implementation, refer to this blog from cp algorithms, where the dual variables $u,v$ are denoted as potential. Hungarian algorithm updates the search of optimal value by continuesly increase the sum of $u,v$: \(f= \sum_{i=1}^n u[i] + \sum_{j=1}^n v[j]\)

In the context of MOT, cost matrix is filled with IOU or similarity score obtained from ReID features between objects in two consecutive frames. Sometimes the matrix is non-square, we can add dummy rows or columes with zeros and run more iterations.

Reference:
[1] Wang, G., Song, M., & Hwang, J. N. (2022). Recent advances in embedding methods for multi-object tracking: a survey. arXiv preprint arXiv:2205.10766.
[2] Boyd, Stephen, and Lieven Vandenberghe. Convex optimization. Cambridge university press, 2004.
[3] Matoušek, Jiří, and Bernd Gärtner. Understanding and using linear programming. Vol. 1. Berlin: Springer, 2007.

Deploy LLMs on a phone with arm chips

4 minute read

Published:

In this blog, I’m going to share how I deployed LLMs (1~3b) and ASR models (whisper tiny & base) on my old smart phone, Xiaomi 8 (dipper). It was released in 2018, with a broken screen and depleted battery after several years of heavy usage, it still offers acceptable speed and fluency running daily light-weight apps. So I wonder, if I deploy modern machine models, such as LLMs, on the phone, and can I run the large models smoothly. Let’s see.
The hardware configuration: 8-cores CPU with 4 big cores (Cortex-A75) and 4 small cores (Cortex-A55), 64 GB storage and 6 GB RAM. It has a integrated GPU, which I didn’t find a way to utilize during model inference, so the computation is totally counted on CPU in this trial. To better leverage the power of this CPU, I uninstall the android system (MIUI 12) and port a Ubuntu touch on the phone. Basically it’s a linux OS but using the underlying andriod framework to control hardwares, and it gives me a much longer battery life compared to android, also more convenience since I am a rookie in android development. Models are quantized versions from llamacpp and whispercpp.
To get rid of the cumbersome work build an app with GUI, I run models in command line using the terminal app originally from Ubuntu touch, which can be regarded as the same terminal on Ubuntu desktop in this case. All I need to do is to compile the c++ code into an executable file that runs on my phone. Since the architecture of my laptop cpu is x86, the version of glibc, libstdc++ are different from the libs on the phone, I could either compile on the phone directly or cross compile on my PC with a specific toolchain. I kept all the heavy work on PC and built my own toolchain using crosstool-NG, which is targeted at building toolchains.

LLMs Inference speed up EP1 - kv cache

6 minute read

Published:

Large Language Models (LLMs) have revolutionized the field of natural language processing, enabling significant advancements in tasks such as language translation, text summarization, and sentiment analysis. However, despite their impressive capabilities, LLMs are not without limitations. One of the most significant challenges facing LLMs today is the problem of inference speed. Due to the sheer size and complexity of these models, the process of making predictions or extracting information from them can be computationally expensive and time-consuming. Several ways to speed up the LLMs without updating hardware:

Customize a local chatbot in streamlit

4 minute read

Published:

Large Language Models (LLMs) are the brains of various chatbots, and luckily we all have access to some brilliant cheap or even free LLMs now, thanks to open source community. It is possible to run LLMs on a PC and keep everything local. This blog presents my solution to building a chatbot running on my PC, with a totally local file storage system and a costumized graphical user interface.