Skip to content

Commit

Permalink
feat: 칼만필터 발열 이슈 수정
Browse files Browse the repository at this point in the history
  • Loading branch information
@MaraMincho
MaraMincho committed Jan 9, 2024
1 parent 11e8928 commit 1f1ce7b
Show file tree
Hide file tree
Showing 5 changed files with 92 additions and 117 deletions.
7 changes: 3 additions & 4 deletions iOS/Projects/Features/Record/Sources/Domain/Entities/KalmanFilterUpdateRequireElement.swift
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,13 +6,12 @@
// Copyright © 2023 kr.codesquad.boostcamp8. All rights reserved.
//

import CoreLocation
import Foundation

// MARK: - KalmanFilterUpdateRequireElement

struct KalmanFilterUpdateRequireElement {
let longitude: Double
let latitude: Double
let prevSpeedAtLongitude: Double
let prevSpeedAtLatitude: Double
let prevCLLocation: CLLocation
let currentCLLocation: CLLocation
}
133 changes: 69 additions & 64 deletions iOS/Projects/Features/Record/Sources/Domain/UseCases/KalmanFilter.swift
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,29 +6,26 @@
// Copyright © 2023 kr.codesquad.boostcamp8. All rights reserved.
//

import CoreLocation
import Foundation
import Log
import simd

struct KalmanFilter {
/// 새로운 값을 입력하게 된다면, 에측을 통해서 값을 작성하게 되는 변수 입니다.
var x = MatrixOfTwoDimension([[]])
var x = simd_double4()

/// 초기 오차 공분산 입니다.
/// 초기 값은 에러가 많기 떄문에 다음과 같이 크게 가져갔습니다.
private var p = MatrixOfTwoDimension([
[500, 0, 0, 0],
private var p = simd_double4x4([
[1, 0, 0, 0],
[0, 1, 0, 0],
[0, 0, 500, 0],
[0, 0, 1, 0],
[0, 0, 0, 1],
])

// 사용자 경험을 통해 얻어진 값 입니다. (일단 대한민국 GPS환경이 좋다고 가정하여,
// 애플 오차의 1/2로 가져갔습니다.)

private var q = MatrixOfTwoDimension([
[0.000455, 0, 0, 0],
private var q = simd_double4x4([
[0.00082, 0, 0, 0],
[0, 0, 0, 0],
[0, 0, 0.000059, 0],
[0, 0, 0.00082, 0],
[0, 0, 0, 0],
])

Expand All @@ -45,77 +42,85 @@ struct KalmanFilter {
/// 1도 일 때 몇 km? = 지구 반지름 6371 * cos(37) * 1 (pi/180) = 85.18km
/// 1º : 85180m = y : 10m
/// y = 0.00011739 ~= 0.000117
private var r = MatrixOfTwoDimension([
[0.000899, 0],
[0, 0.000117],
private var r = simd_double2x2([
[0.00082, 0],
[0, 0.00082],
])

var prevHeadingValue: Double
var prevSpeedAtLatitude: Double = 0
var prevSpeedAtLongitude: Double = 0

/// 관계 식 입니다.
lazy var A = MatrixOfTwoDimension([
[1, cos(prevHeadingValue) * prevSpeedAtLatitude, 0, 0],
lazy var A = simd_double4x4([
[1, timeInterval * prevVelocity.lat, 0, 0],
[0, 1, 0, 0],
[0, 0, 1, sin(prevHeadingValue) * prevSpeedAtLongitude],
[0, 0, 1, timeInterval * prevVelocity.long],
[0, 0, 0, 1],
])

/// 우리가 궁금한건 위도와 경도이기 때문에 필요한 부분만 기재했습니다.
private var H = MatrixOfTwoDimension([
let H = simd_double4x2([
[1, 0],
[0, 0],
[0, 1],
[0, 0],
])

// 우리가 궁금한건 위도와 경도이기 때문에 필요한 부분만 기재했습니다.

var prevTime: Date
init(initLocation: CLLocation) {
x = .init(initLocation.coordinate.latitude, 0, initLocation.coordinate.longitude, 0)
prevTime = initLocation.timestamp
}

var timeInterval: Double = 0.0

var prevLocation = CLLocation()

let pIdentity = simd_double4x4([
[1, 0, 0, 0],
[0, 1, 0, 0],
[0, 0, 1, 0],
[0, 0, 0, 1],
])

init(initLongitude: Double, initLatitude: Double, headingValue: Double) {
x = .init([
[initLatitude],
[0],
[initLongitude],
[0],
])
prevHeadingValue = headingValue
}
var prevVelocity: (lat: Double, long: Double) = (0, 0)

/// 사용자가 가르키는 방향을 업데이트 합니다.
mutating func update(heading: Double) {
prevHeadingValue = heading
}
mutating func update(currentLocation: CLLocation) {
let currentTime = currentLocation.timestamp

let prevTimeInterval = prevTime.timeIntervalSince1970
let currentTimeInterval = currentTime.timeIntervalSince1970

timeInterval = currentTimeInterval - prevTimeInterval

let velocityLatitude = (prevLocation.coordinate.latitude - currentLocation.coordinate.latitude)
let velocityLongitude = (prevLocation.coordinate.longitude - currentLocation.coordinate.longitude)

/// Update합니다.
mutating func update(initLongitude: Double, initLatitude: Double, prevSpeedAtLatitude: Double, prevSpeedAtLongitude: Double) {
let mesure = MatrixOfTwoDimension(
prevVelocity = (velocityLatitude, velocityLongitude)

prevLocation = currentLocation
prevTime = currentTime

let mesure = simd_double2(
[
[initLatitude],
[initLongitude],
currentLocation.coordinate.latitude,
currentLocation.coordinate.longitude,
]
)
self.prevSpeedAtLatitude = prevSpeedAtLatitude
self.prevSpeedAtLongitude = prevSpeedAtLongitude
guard
let prediction = A.multiply(x),
let predictionErrorCovariance = A.multiply(p)?.multiply(A.transPose())?.add(q),

let notInversed = H.multiply(predictionErrorCovariance)?.multiply(H.transPose())?.add(r),
let prevKalman = notInversed.invert(),
let kalman = predictionErrorCovariance.multiply(H.transPose())?.multiply(prevKalman),

let tempValue = H.multiply(prediction),
let subTractedValue = mesure.sub(tempValue),
let multiedKalmanValue = kalman.multiply(subTractedValue),
let currentX = prediction.add(multiedKalmanValue),

let tempPredictionErrorCovariance = kalman.multiply(H)?.multiply(predictionErrorCovariance),
let currentPredictionErrorCovariance = predictionErrorCovariance.sub(tempPredictionErrorCovariance)
else {
return
}

let xp = A * x
let pp = A * p * A.transpose + q

let temp = pp * H.transpose
let invert = (H * pp * H.transpose + r).inverse
let kalman = temp * invert

let currentX = xp + kalman * (mesure - H * xp)

let currentP = pp - kalman * H * pp.inverse
x = currentX
p = currentPredictionErrorCovariance
p = currentP
}

var latestCensoredPosition: KalmanFilterCensored {
return .init(longitude: x.value[2][0], latitude: x.value[0][0])
return .init(longitude: x[2], latitude: x[0])
}
}
15 changes: 3 additions & 12 deletions iOS/Projects/Features/Record/Sources/Domain/UseCases/KalmanUseCase.swift
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,7 +13,6 @@ import Foundation

protocol KalmanUseCaseRepresentable {
func updateFilter(_ element: KalmanFilterUpdateRequireElement) -> KalmanFilterCensored?
func updateHeading(_ heading: Double)
}

// MARK: - KalmanUseCase
Expand All @@ -27,21 +26,13 @@ final class KalmanUseCase {
// MARK: KalmanUseCaseRepresentable

extension KalmanUseCase: KalmanUseCaseRepresentable {
func updateHeading(_ heading: Double) {
filter?.update(heading: heading)
}

func updateFilter(_ element: KalmanFilterUpdateRequireElement) -> KalmanFilterCensored? {
if filter == nil {
filter = .init(initLongitude: element.latitude, initLatitude: element.longitude, headingValue: 0)
let currentLocation = element.currentCLLocation
filter = .init(initLocation: currentLocation)
return nil
}
filter?.update(
initLongitude: element.longitude,
initLatitude: element.latitude,
prevSpeedAtLatitude: element.prevSpeedAtLatitude,
prevSpeedAtLongitude: element.prevSpeedAtLongitude
)
filter?.update(currentLocation: element.currentCLLocation)

return filter?.latestCensoredPosition
}
Expand Down
45 changes: 16 additions & 29 deletions ...s/Presentation/WorkoutSessionGroupScene/RouteMapScene/WorkoutRouteMapViewController.swift
View file
Original file line number Diff line number Diff line change
Expand Up @@ -35,13 +35,12 @@ final class WorkoutRouteMapViewController: UIViewController {
private let viewModel: WorkoutRouteMapViewModelRepresentable

/// 사용자 위치 추적 배열
@Published private var locations: [CLLocation] = []
@Published private var locations: [CLLocationCoordinate2D] = []

private let mapCaptureDataSubject: PassthroughSubject<Data?, Never> = .init()
private let mapSnapshotterImageDataSubject: PassthroughSubject<[CLLocation], Never> = .init()

private let kalmanFilterShouldUpdatePositionSubject: PassthroughSubject<KalmanFilterUpdateRequireElement, Never> = .init()
private let kalmanFilterShouldUpdateHeadingSubject: PassthroughSubject<Double, Never> = .init()

private var subscriptions: Set<AnyCancellable> = []

Expand Down Expand Up @@ -137,7 +136,6 @@ final class WorkoutRouteMapViewController: UIViewController {

let input: WorkoutRouteMapViewModelInput = .init(
filterShouldUpdatePositionPublisher: kalmanFilterShouldUpdatePositionSubject.eraseToAnyPublisher(),
filterShouldUpdateHeadingPublisher: kalmanFilterShouldUpdateHeadingSubject.eraseToAnyPublisher(),
locationListPublisher: locationPublisher
)

Expand Down Expand Up @@ -167,8 +165,7 @@ final class WorkoutRouteMapViewController: UIViewController {
return
}

let coordinates = locations.map(\.coordinate)
let polyLine = MKPolyline(coordinates: coordinates, count: coordinates.count)
let polyLine = MKPolyline(coordinates: locations, count: locations.count)
let span = MKCoordinateSpan(
latitudeDelta: (regionData.maxLatitude - regionData.minLatitude) * 1.15,
longitudeDelta: (regionData.maxLongitude - regionData.minLongitude) * 1.15
Expand Down Expand Up @@ -210,8 +207,8 @@ final class WorkoutRouteMapViewController: UIViewController {
self?.locations
.forEach { location in
let currentCLLocationCoordinator2D = CLLocationCoordinate2D(
latitude: location.coordinate.latitude,
longitude: location.coordinate.longitude
latitude: location.latitude,
longitude: location.longitude
)

// snapshot에서 현재 위도 경도에 대한 데이터가 어느 CGPoint에 있는지 찾아내고, 이를 Polyline을 그립니다.
Expand All @@ -232,9 +229,8 @@ final class WorkoutRouteMapViewController: UIViewController {
}

let currentLocation = CLLocation(latitude: value.latitude, longitude: value.longitude)
locations.append(currentLocation)
let coordinates = locations.map(\.coordinate)
let polyline = MKPolyline(coordinates: coordinates, count: coordinates.count)
locations.append(currentLocation.coordinate)
let polyline = MKPolyline(coordinates: locations, count: locations.count)

mapView.removeOverlays(mapView.overlays)
mapView.addOverlay(polyline)
Expand All @@ -260,6 +256,7 @@ final class WorkoutRouteMapViewController: UIViewController {

extension WorkoutRouteMapViewController: LocationTrackingProtocol {
func requestCapture() {
let locations = locations.map { CLLocation(latitude: $0.latitude, longitude: $0.longitude) }
mapSnapshotterImageDataSubject.send(locations)
}

Expand All @@ -268,7 +265,12 @@ extension WorkoutRouteMapViewController: LocationTrackingProtocol {
}

var locationPublisher: AnyPublisher<[CLLocation], Never> {
$locations.eraseToAnyPublisher()
return Just(
locations.map {
CLLocation(latitude: $0.latitude, longitude: $0.longitude)
}
)
.eraseToAnyPublisher()
}
}

Expand All @@ -293,28 +295,13 @@ extension WorkoutRouteMapViewController: CLLocationManagerDelegate {
return
}

let currentTime = Date.now
let timeDistance = currentTime.distance(to: prevDate)

// 과거 위치와 현재 위치를 통해 위 경도에 관한 속력을 구합니다.
let v = (
(newLocation.coordinate.latitude - prevLocation.coordinate.latitude) / timeDistance,
(newLocation.coordinate.longitude - prevLocation.coordinate.longitude) / timeDistance
)
prevLocation = newLocation

kalmanFilterShouldUpdatePositionSubject.send(
.init(
longitude: newLocation.coordinate.longitude,
latitude: newLocation.coordinate.latitude,
prevSpeedAtLongitude: v.1,
prevSpeedAtLatitude: v.0
prevCLLocation: prevLocation,
currentCLLocation: newLocation
)
)
}

func locationManager(_: CLLocationManager, didUpdateHeading newHeading: CLHeading) {
kalmanFilterShouldUpdateHeadingSubject.send(newHeading.trueHeading)
prevLocation = newLocation
}

func locationManager(_: CLLocationManager, didFailWithError error: Error) {
Expand Down
9 changes: 1 addition & 8 deletions ...ources/Presentation/WorkoutSessionGroupScene/RouteMapScene/WorkoutRouteMapViewModel.swift
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,7 +13,6 @@ import Foundation

public struct WorkoutRouteMapViewModelInput {
let filterShouldUpdatePositionPublisher: AnyPublisher<KalmanFilterUpdateRequireElement, Never>
let filterShouldUpdateHeadingPublisher: AnyPublisher<Double, Never>
let locationListPublisher: AnyPublisher<[LocationModel], Never>
}

Expand Down Expand Up @@ -58,13 +57,6 @@ extension WorkoutRouteMapViewModel: WorkoutRouteMapViewModelRepresentable {
public func transform(input: WorkoutRouteMapViewModelInput) -> WorkoutRouteMapViewModelOutput {
subscriptions.removeAll()

input
.filterShouldUpdateHeadingPublisher
.sink { [kalmanUseCase] value in
kalmanUseCase.updateHeading(value)
}
.store(in: &subscriptions)

let region = input
.locationListPublisher
.map(locationPathUseCase.processPath(locations:))
Expand All @@ -73,6 +65,7 @@ extension WorkoutRouteMapViewModel: WorkoutRouteMapViewModelRepresentable {
let updateValue: WorkoutRouteMapViewModelOutput = input
.filterShouldUpdatePositionPublisher
.dropFirst(4)
.throttle(for: 1, scheduler: RunLoop.main, latest: false)
.map { [kalmanUseCase] element in
let censoredValue = kalmanUseCase.updateFilter(element)
return WorkoutRouteMapState.censoredValue(censoredValue)
Expand Down

0 comments on commit 1f1ce7b

Please sign in to comment.