Comparison of KF-Based Vehicle Sideslip Estimation Logics with Increasing Complexity for a Passenger Car

. 2024 Jul 25;24(15):4846. doi: 10.3390/s24154846

Algorithm 1 Iterated Extended Kalman Filter Algorithm

1:
${\hat{x}}_{0}$ ▹ Define initial state
2:
$P_{0}$ ▹ Define initial state covariance
3:
for $k = 1$ to T do
4:
${\hat{x}}_{k}^{-} = f ({\hat{x}}_{k - 1}, u_{k - 1}, 0)$ ▹ a priori state estimate
5:
$P_{k}^{-} = A_{k - 1} P_{k - 1} A_{k - 1}^{T} + Q$ ▹ a priori state estimate covariance
6:
${\hat{x}}_{k, 1} = {\hat{x}}_{k}^{-}$
7:
for $i = 1$ to N do
8:
$z_{k, i} = h ({\hat{x}}_{k, i}, u_{k}, 0) - H_{k, i} ({\hat{x}}_{k}^{-} - {\hat{x}}_{k, i})$ ▹ a priori measurement estimate
9:
$K_{k, i} = P_{k}^{-} H_{k, i}^{T} {(H_{k, i} P_{k}^{-} H_{k, i}^{T} + R)}^{- 1}$ ▹ Kalman gain
10:
${\hat{x}}_{k, i + 1} = {\hat{x}}_{k}^{-} + K_{k, i} (y_{k} - z_{k, i})$ ▹ a posteriori state estimate
11:
$P_{k, i + 1} = (I - K_{k, i} H_{k, i}) P_{k}^{-}$ ▹ a posteriori state estimate covariance
12:
end for
13:
end for